RobotLocomotion · jwnimmer-tri · Mar 3, 2025 · Mar 3, 2025
diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc
@@ -3895,8 +3895,8 @@ void MultibodyPlant<T>::CalcReactionForces(
   // for the discrete solvers we have today, though it might change for future
   // solvers that prefer an implicit evaluation of these terms.
   // TODO(amcastro-tri): Consider having a
-  //  DiscreteUpdateManager::EvalReactionForces() to ensure the manager performs
-  //  this computation consistently with its discrete update.
+  // DiscreteUpdateManager::EvalReactionForces() to ensure the manager performs
+  // this computation consistently with its discrete update.
   const VectorX<T>& vdot = this->EvalForwardDynamics(context).get_vdot();
   std::vector<SpatialAcceleration<T>> A_WB_vector(num_bodies());
   std::vector<SpatialForce<T>> F_BMo_W_vector(num_bodies());
@@ -3908,9 +3908,9 @@ void MultibodyPlant<T>::CalcReactionForces(
   // Since vdot is the result of Fapplied and tau_applied we expect the result
   // from inverse dynamics to be zero.
   // TODO(amcastro-tri): find a better estimation for this bound. For instance,
-  //  we can make an estimation based on the trace of the mass matrix (Jain
-  //  2011, Eq. 4.21). For now we only ASSERT though with a better estimation we
-  //  could promote this to a DEMAND.
+  // we can make an estimation based on the trace of the mass matrix (Jain 2011,
+  // Eq. 4.21). For now we only ASSERT though with a better estimation we could
+  // promote this to a DEMAND.
   // TODO(amcastro-tri) Uncomment this line once issue #12473 is resolved.
   // DRAKE_ASSERT(tau_id.norm() <
   //              100 * num_velocities() *
@@ -3924,49 +3924,61 @@ void MultibodyPlant<T>::CalcReactionForces(
         internal_tree().get_joint_mobilizer(joint_index);
     const internal::Mobilizer<T>& mobilizer =
         internal_tree().get_mobilizer(mobilizer_index);
-
-    // Reversed means the joint's parent(child) body is the outboard(inboard)
-    // body for the mobilizer.
-    const bool is_reversed = mobilizer.mobod().is_reversed();
+    const internal::MobodIndex mobod_index = mobilizer.mobod().index();
 
     // F_BMo_W is the mobilizer reaction force on mobilized body B at the origin
     // Mo of the mobilizer's outboard frame M, expressed in the world frame W.
-    const SpatialForce<T>& F_BMo_W = F_BMo_W_vector[mobilizer_index];
-
-    // But the quantity of interest, F_CJc_Jc, is the joint's reaction force on
-    // the joint's child body C at the joint's child frame Jc, expressed in Jc.
-    SpatialForce<T>& F_CJc_Jc = output->at(joint.ordinal());
+    const SpatialForce<T>& F_BMo_W = F_BMo_W_vector[mobod_index];
 
     // Frames of interest:
+    const Frame<T>& frame_Jp = joint.frame_on_parent();
     const Frame<T>& frame_Jc = joint.frame_on_child();
-    const Frame<T>& frame_M = mobilizer.outboard_frame();
+    const FrameIndex F_index = mobilizer.inboard_frame().index();
+    const FrameIndex M_index = mobilizer.outboard_frame().index();
+    const FrameIndex Jp_index = frame_Jp.index();
+    const FrameIndex Jc_index = frame_Jc.index();
+
+    // In Drake we must have either:
+    //  - Jp == F and Jc == M (typical case)
+    //  - Jp == M and Jc == F (mobilizer is reversed from joint)
+    DRAKE_DEMAND((Jp_index == F_index && Jc_index == M_index) ||
+                 (Jp_index == M_index && Jc_index == F_index));
+
+    // Mobilizer is reversed if the joint's parent frame Jp is the mobilizer's
+    // outboard frame M.
+    const bool is_reversed = (Jp_index == M_index);
 
     // We'll need this in both cases below since we're required to report
     // the reaction force expressed in the joint's child frame Jc.
     const RotationMatrix<T> R_JcW =
         frame_Jc.CalcRotationMatrixInWorld(context).inverse();
 
-    if (&frame_M == &frame_Jc) {
-      // This is the easy case. Just need to re-express.
-      F_CJc_Jc = R_JcW * F_BMo_W;
-      continue;
-    }
+    // The quantity of interest, F_CJc_Jc, is the joint's reaction force on the
+    // joint's child body C at the joint's child frame Jc, expressed in Jc.
+    SpatialForce<T>& F_CJc_Jc = output->at(joint.ordinal());
+    if (!is_reversed) {
+      F_CJc_Jc = R_JcW * F_BMo_W;  // The typical case: Mo==Jc and B==C.
+    } else {
+      // For this reversed case, F_BMo_W is the reaction on the joint's _parent_
+      // body at Jp, expressed in W.
+      const SpatialForce<T>& F_PJp_W = F_BMo_W;  // Reversed: Mo==Jp and B==P.
 
-    // If the mobilizer is reversed, Newton's 3ʳᵈ law (action/reaction) (and
-    // knowing Drake's joints are massless) says the force on the child at M
-    // is equal and opposite to the force on the parent at M.
-    const SpatialForce<T> F_CMo_W = is_reversed ? -F_BMo_W : F_BMo_W;
-    const SpatialForce<T> F_CMo_Jc = R_JcW * F_CMo_W;  // Reexpress in Jc.
+      // Newton's 3ʳᵈ law (action/reaction) (and knowing Drake's joints are
+      // massless) says the force on the child _at Jp_ is equal and opposite to
+      // the force on the parent at Jp.
+      const SpatialForce<T> F_CJp_W = -F_PJp_W;
+      const SpatialForce<T> F_CJp_Jc = R_JcW * F_CJp_W;  // Reexpress in Jc.
 
-    // However, the reaction force we want to report on the child is at Jc,
-    // not M. We need to shift the application point from Mo to Jco.
+      // However, the reaction force we want to report on the child is at Jc,
+      // not Jp. We need to shift the application point from Jp to Jc.
 
-    // Find the shift vector p_MoJco_Jc (= -p_JcoMo_Jc).
-    const RigidTransform<T> X_JcM = frame_M.CalcPose(context, frame_Jc);
-    const Vector3<T> p_MoJco_Jc = -X_JcM.translation();
+      // Find the shift vector p_JpJc_Jc (= -p_JcJp_Jc).
+      const RigidTransform<T> X_JcJp = frame_Jp.CalcPose(context, frame_Jc);
+      const Vector3<T> p_JpJc_Jc = -X_JcJp.translation();
 
-    // Perform  the M->Jc shift.
-    F_CJc_Jc = F_CMo_Jc.Shift(p_MoJco_Jc);
+      // Perform  the Jp->Jc shift.
+      F_CJc_Jc = F_CJp_Jc.Shift(p_JpJc_Jc);
+    }
   }
 }
 

diff --git a/multibody/plant/test/sap_driver_multidof_joints_test.cc b/multibody/plant/test/sap_driver_multidof_joints_test.cc
@@ -92,15 +92,13 @@ class MultiDofJointWithLimits final : public Joint<T> {
   int do_get_position_start() const override { return 0; }
 
   std::unique_ptr<Mobilizer<T>> MakeMobilizerForJoint(
-      const SpanningForest::Mobod& mobod,
-      MultibodyTree<T>* tree) const override {
-    DRAKE_DEMAND(tree != nullptr);  // Just a sanity check; we don't need it.
+      const SpanningForest::Mobod& mobod) const override {
     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().
+    // 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>>(
         mobod, *inboard_frame, *outboard_frame);
     return revolute_mobilizer;

diff --git a/multibody/rational/rational_forward_kinematics.cc b/multibody/rational/rational_forward_kinematics.cc
@@ -179,11 +179,10 @@ RationalForwardKinematics::Pose<T> RationalForwardKinematics::
 template <typename T>
 RationalForwardKinematics::Pose<T>
 RationalForwardKinematics::CalcWeldJointChildBodyPose(
-    const math::RigidTransformd& X_PF, const math::RigidTransformd& X_MC,
-    const Pose<T>& X_AP) const {
-  // X_FM is always identity for a Weld mobilizer.
-  const Matrix3<double> R_FM = Matrix3<double>::Identity();
-  const Vector3<double> p_FM = Vector3<double>::Zero();
+    const math::RigidTransformd& X_FM, const math::RigidTransformd& X_PF,
+    const math::RigidTransformd& X_MC, const Pose<T>& X_AP) const {
+  const Matrix3<double> R_FM = X_FM.rotation().matrix();
+  const Vector3<double> p_FM = X_FM.translation();
   const Matrix3<T>& R_AP = X_AP.rotation;
   const Vector3<T>& p_AP = X_AP.position;
   return CalcChildPose(R_AP, p_AP, X_PF, X_MC, R_FM, p_FM);
@@ -288,9 +287,17 @@ RationalForwardKinematics::CalcChildBodyPoseAsMultilinearPolynomial(
     return CalcPrismaticJointChildLinkPose(axis_F, X_PF, X_MC, X_AP,
                                            q_star(s_index), s_[s_index]);
   } else if (IsWeld(*mobilizer)) {
-    return CalcWeldJointChildBodyPose(X_PF, X_MC, X_AP);
+    const internal::WeldMobilizer<double>* weld_mobilizer =
+        static_cast<const internal::WeldMobilizer<double>*>(mobilizer);
+    math::RigidTransformd X_FM;
+    if (!is_order_reversed) {
+      X_FM = weld_mobilizer->get_X_FM();
+    } else {
+      X_FM = weld_mobilizer->get_X_FM().inverse();
+    }
+    return CalcWeldJointChildBodyPose(X_FM, X_PF, X_MC, X_AP);
   }
-  // Successful construction guarantees that all supported mobilizers are
+  // Successful construction guarantess that all supported mobilizers are
   // handled.
   DRAKE_UNREACHABLE();
 }

diff --git a/multibody/rational/rational_forward_kinematics.h b/multibody/rational/rational_forward_kinematics.h
@@ -233,7 +233,8 @@ class RationalForwardKinematics {
   // Computes the pose of the body, connected to its parent body through a
   // weld joint.
   template <typename T>
-  Pose<T> CalcWeldJointChildBodyPose(const math::RigidTransformd& X_PF,
+  Pose<T> CalcWeldJointChildBodyPose(const math::RigidTransformd& X_FM,
+                                     const math::RigidTransformd& X_PF,
                                      const math::RigidTransformd& X_MC,
                                      const Pose<T>& X_AP) const;
 

diff --git a/multibody/tree/ball_rpy_joint.cc b/multibody/tree/ball_rpy_joint.cc
@@ -71,8 +71,7 @@ std::unique_ptr<Joint<T>> BallRpyJoint<T>::DoShallowClone() const {
 // in the header file.
 template <typename T>
 std::unique_ptr<internal::Mobilizer<T>> BallRpyJoint<T>::MakeMobilizerForJoint(
-    const internal::SpanningForest::Mobod& mobod,
-    internal::MultibodyTree<T>*) const {
+    const internal::SpanningForest::Mobod& mobod) const {
   const auto [inboard_frame, outboard_frame] =
       this->tree_frames(mobod.is_reversed());
   // TODO(sherm1) The mobilizer needs to be reversed, not just the frames.

diff --git a/multibody/tree/ball_rpy_joint.h b/multibody/tree/ball_rpy_joint.h
@@ -71,9 +71,9 @@ class BallRpyJoint final : public Joint<T> {
     DRAKE_THROW_UNLESS(damping >= 0);
   }
 
-  ~BallRpyJoint() final;
+  ~BallRpyJoint() override;
 
-  const std::string& type_name() const final;
+  const std::string& type_name() const override;
 
   /// Returns `this` joint's default damping constant in N⋅m⋅s. The damping
   /// torque (in N⋅m) is modeled as `τ = -damping⋅ω`, i.e. opposing motion, with
@@ -191,7 +191,7 @@ class BallRpyJoint final : public Joint<T> {
   /// Adding forces per-dof makes no physical sense. Therefore, this method
   /// throws an exception if invoked.
   void DoAddInOneForce(const systems::Context<T>&, int, const T&,
-                       MultibodyForces<T>*) const final {
+                       MultibodyForces<T>*) const override {
     throw std::logic_error(
         "Ball RPY joints do not allow applying forces to individual degrees of "
         "freedom.");
@@ -203,7 +203,7 @@ class BallRpyJoint final : public Joint<T> {
   /// viscous law `τ = -d⋅ω`, with d the damping coefficient (see
   /// default_damping()).
   void DoAddInDamping(const systems::Context<T>& context,
-                      MultibodyForces<T>* forces) const final {
+                      MultibodyForces<T>* forces) const override {
     Eigen::Ref<VectorX<T>> t_BMo_F =
         get_mobilizer().get_mutable_generalized_forces_from_array(
             &forces->mutable_generalized_forces());
@@ -212,48 +212,47 @@ class BallRpyJoint final : public Joint<T> {
   }
 
  private:
-  int do_get_velocity_start() const final {
+  int do_get_velocity_start() const override {
     return get_mobilizer().velocity_start_in_v();
   }
 
-  int do_get_num_velocities() const final { return 3; }
+  int do_get_num_velocities() const override { return 3; }
 
-  int do_get_position_start() const final {
+  int do_get_position_start() const override {
     return get_mobilizer().position_start_in_q();
   }
 
-  int do_get_num_positions() const final { return 3; }
+  int do_get_num_positions() const override { return 3; }
 
-  std::string do_get_position_suffix(int index) const final {
+  std::string do_get_position_suffix(int index) const override {
     return get_mobilizer().position_suffix(index);
   }
 
-  std::string do_get_velocity_suffix(int index) const final {
+  std::string do_get_velocity_suffix(int index) const override {
     return get_mobilizer().velocity_suffix(index);
   }
 
   void do_set_default_positions(
-      const VectorX<double>& default_positions) final {
+      const VectorX<double>& default_positions) override {
     if (this->has_mobilizer()) {
       get_mutable_mobilizer().set_default_position(default_positions);
     }
   }
 
   // Joint<T> overrides:
   std::unique_ptr<internal::Mobilizer<T>> MakeMobilizerForJoint(
-      const internal::SpanningForest::Mobod& mobod,
-      internal::MultibodyTree<T>* tree) const final;
+      const internal::SpanningForest::Mobod& mobod) const override;
 
   std::unique_ptr<Joint<double>> DoCloneToScalar(
-      const internal::MultibodyTree<double>& tree_clone) const final;
+      const internal::MultibodyTree<double>& tree_clone) const override;
 
   std::unique_ptr<Joint<AutoDiffXd>> DoCloneToScalar(
-      const internal::MultibodyTree<AutoDiffXd>& tree_clone) const final;
+      const internal::MultibodyTree<AutoDiffXd>& tree_clone) const override;
 
   std::unique_ptr<Joint<symbolic::Expression>> DoCloneToScalar(
-      const internal::MultibodyTree<symbolic::Expression>&) const final;
+      const internal::MultibodyTree<symbolic::Expression>&) const override;
 
-  std::unique_ptr<Joint<T>> DoShallowClone() const final;
+  std::unique_ptr<Joint<T>> DoShallowClone() const override;
 
   // Make BallRpyJoint templated on every other scalar type a friend of
   // BallRpyJoint<T> so that CloneToScalar<ToAnyOtherScalar>() can access

diff --git a/multibody/tree/curvilinear_joint.cc b/multibody/tree/curvilinear_joint.cc
@@ -101,8 +101,7 @@ std::unique_ptr<Joint<T>> CurvilinearJoint<T>::DoShallowClone() const {
 template <typename T>
 std::unique_ptr<internal::Mobilizer<T>>
 CurvilinearJoint<T>::MakeMobilizerForJoint(
-    const internal::SpanningForest::Mobod& mobod,
-    internal::MultibodyTree<T>*) const {
+    const internal::SpanningForest::Mobod& mobod) const {
   const auto [inboard_frame, outboard_frame] =
       this->tree_frames(mobod.is_reversed());
   // TODO(sherm1) The mobilizer needs to be reversed, not just the frames.

diff --git a/multibody/tree/curvilinear_joint.h b/multibody/tree/curvilinear_joint.h
@@ -121,9 +121,9 @@ class CurvilinearJoint final : public Joint<T> {
           curvilinear_path,
       double pos_lower_limit, double pos_upper_limit, double damping = 0);
 
-  ~CurvilinearJoint() final;
+  ~CurvilinearJoint() override;
 
-  const std::string& type_name() const final;
+  const std::string& type_name() const override;
 
   /** Returns `this` joint's default damping constant in N⋅s/m. */
   double default_damping() const { return this->default_damping_vector()[0]; }
@@ -269,7 +269,7 @@ class CurvilinearJoint final : public Joint<T> {
    @see The public NVI AddInOneForce() for details. */
   void DoAddInOneForce(const systems::Context<T>&, int joint_dof,
                        const T& joint_tau,
-                       MultibodyForces<T>* forces) const final {
+                       MultibodyForces<T>* forces) const override {
     DRAKE_DEMAND(joint_dof == 0);
     Eigen::Ref<VectorX<T>> tau_mob =
         get_mobilizer().get_mutable_generalized_forces_from_array(
@@ -288,63 +288,62 @@ class CurvilinearJoint final : public Joint<T> {
    @param[out] forces The MultibodyForces object to which the damping force is
    added. */
   void DoAddInDamping(const systems::Context<T>& context,
-                      MultibodyForces<T>* forces) const final {
+                      MultibodyForces<T>* forces) const override {
     const T damping_force =
         -this->GetDamping(context) * get_tangential_velocity(context);
     AddInForce(context, damping_force, forces);
   }
 
  private:
-  int do_get_velocity_start() const final {
+  int do_get_velocity_start() const override {
     return get_mobilizer().velocity_start_in_v();
   }
 
-  int do_get_num_velocities() const final { return 1; }
+  int do_get_num_velocities() const override { return 1; }
 
-  int do_get_position_start() const final {
+  int do_get_position_start() const override {
     return get_mobilizer().position_start_in_q();
   }
 
-  int do_get_num_positions() const final { return 1; }
+  int do_get_num_positions() const override { return 1; }
 
-  std::string do_get_position_suffix(int index) const final {
+  std::string do_get_position_suffix(int index) const override {
     return get_mobilizer().position_suffix(index);
   }
 
-  std::string do_get_velocity_suffix(int index) const final {
+  std::string do_get_velocity_suffix(int index) const override {
     return get_mobilizer().velocity_suffix(index);
   }
 
   void do_set_default_positions(
-      const VectorX<double>& default_positions) final {
+      const VectorX<double>& default_positions) override {
     if (this->has_mobilizer()) {
       get_mutable_mobilizer().set_default_position(default_positions);
     }
   }
 
-  const T& DoGetOnePosition(const systems::Context<T>& context) const final {
+  const T& DoGetOnePosition(const systems::Context<T>& context) const override {
     return get_distance(context);
   }
 
-  const T& DoGetOneVelocity(const systems::Context<T>& context) const final {
+  const T& DoGetOneVelocity(const systems::Context<T>& context) const override {
     return get_tangential_velocity(context);
   }
 
   // Joint<T> overrides:
   std::unique_ptr<internal::Mobilizer<T>> MakeMobilizerForJoint(
-      const internal::SpanningForest::Mobod& mobod,
-      internal::MultibodyTree<T>* tree) const final;
+      const internal::SpanningForest::Mobod& mobod) const override;
 
   std::unique_ptr<Joint<double>> DoCloneToScalar(
-      const internal::MultibodyTree<double>& tree_clone) const final;
+      const internal::MultibodyTree<double>& tree_clone) const override;
 
   std::unique_ptr<Joint<AutoDiffXd>> DoCloneToScalar(
-      const internal::MultibodyTree<AutoDiffXd>& tree_clone) const final;
+      const internal::MultibodyTree<AutoDiffXd>& tree_clone) const override;
 
   std::unique_ptr<Joint<symbolic::Expression>> DoCloneToScalar(
-      const internal::MultibodyTree<symbolic::Expression>&) const final;
+      const internal::MultibodyTree<symbolic::Expression>&) const override;
 
-  std::unique_ptr<Joint<T>> DoShallowClone() const final;
+  std::unique_ptr<Joint<T>> DoShallowClone() const override;
 
   // Make CurvilinearJoint templated on every other scalar type a friend of
   // CurvilinearJoint<T> so that CloneToScalar<ToAnyOtherScalar>() can access