Working on mass matrix in M

sherm1 · sherm1 · commit 7f188e925f39 · 2025-05-15T16:05:49.000-07:00
diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h
@@ -4337,6 +4337,15 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem<T> {
     internal_tree().CalcMassMatrix(context, M);
   }
 
+  // Same mass matrix, calculated by working in each body's inboard mobilizer
+  // frame M, rather than in World. Should be faster.
+  void CalcMassMatrixInM(const systems::Context<T>& context,
+                         EigenPtr<MatrixX<T>> M) const {
+    this->ValidateContext(context);
+    DRAKE_DEMAND(M != nullptr);
+    internal_tree().CalcMassMatrixInM(context, M);
+  }
+
   /// This method allows users to map the state of `this` model, x, into a
   /// vector of selected state xₛ with a given preferred ordering.
   /// The mapping, or selection, is returned in the form of a selector matrix
diff --git a/multibody/tree/body_node.cc b/multibody/tree/body_node.cc
@@ -85,10 +85,10 @@ void BodyNode<T>::CalcCompositeBodyInertiaInM_TipToBase(
     const math::RigidTransform<T>& X_MbMc = pcm.get_X_MpM(child_node_index);
     // Shift the child's composite body inertia to B's Mb frame origin Mbo, but
     // still expressed in Mc (so Mx==Mc here).
-    const Vector3<T> p_McMb_Mb = -X_MbMc.translation();  // 3 flops
+    const Vector3<T> p_McMb_Mb = -X_MbMc.translation();      // 3 flops
     SpatialInertia<T> I_CMb_Mx = I_CMc_Mc.Shift(p_McMb_Mb);  // 37 flops
     I_CMb_Mx.ReExpressInPlace(X_MbMc.rotation());  // Now Mx==Mb, 72 flops
-    I_BMb_Mb += I_CMb_Mx;  // 36 flops
+    I_BMb_Mb += I_CMb_Mx;                          // 36 flops
   }
 }
 
diff --git a/multibody/tree/body_node.h b/multibody/tree/body_node.h
@@ -305,12 +305,18 @@ class BodyNode : public MultibodyElement<T> {
   // sweep down to World filling in its diagonal contributions as in the
   // inner loop of algorithm 9.3, using the appropriate
   // CalcMassMatrixOffDiagonalHelper().
-  virtual void CalcMassMatrixContribution_TipToBase(
+  virtual void CalcMassMatrixContributionInWorld_TipToBase(
       const PositionKinematicsCache<T>& pc,
-      const std::vector<SpatialInertia<T>>& Mc_B_W_cache,
+      const std::vector<SpatialInertia<T>>& I_BBo_W_cache,
       const std::vector<Vector6<T>>& H_PB_W_cache,
       EigenPtr<MatrixX<T>> M) const = 0;
 
+  // Same, but calculated in body M frames rather than World.
+  virtual void CalcMassMatrixContributionInM_TipToBase(
+      const PositionKinematicsCacheInM<T>& pcm,
+      const std::vector<SpatialInertia<T>>& I_BMo_M_cache,
+      EigenPtr<MatrixX<T>> M) const = 0;
+
   // There are six functions for calculating the off-diagonal blocks, one for
   // each possible size Rnv of body R(k)'s inboard mobilizer (welds don't
   // contribute here). This allows us to use fixed-size 2d matrices in the
@@ -724,20 +730,20 @@ class BodyNode : public MultibodyElement<T> {
 };
 
 // During mass matrix computation, this dispatcher is invoked by the
-// composite body R(k) on each of the bodies on the path to World.
-template <typename T, int Rnv>
+// composite body B on each of the bodies on its inboard path to World.
+template <typename T, int Bnv>
 class CalcMassMatrixOffDiagonalDispatcher;
 
-#define SPECIALIZE_MASS_MATRIX_DISPATCHER(Rnv)                           \
+#define SPECIALIZE_MASS_MATRIX_DISPATCHER(Bnv)                           \
   template <typename T>                                                  \
-  class CalcMassMatrixOffDiagonalDispatcher<T, Rnv> {                    \
+  class CalcMassMatrixOffDiagonalDispatcher<T, Bnv> {                    \
    public:                                                               \
-    static void Dispatch(const BodyNode<T>& body_node, int R_start_in_v, \
+    static void Dispatch(const BodyNode<T>& body_node, int B_start_in_v, \
                          const std::vector<Vector6<T>>& H_PB_W_cache,    \
-                         const Eigen::Matrix<T, 6, Rnv>& Fm_CBo_W,       \
+                         const Eigen::Matrix<T, 6, Bnv>& Fm_CPo_W,       \
                          EigenPtr<MatrixX<T>> M) {                       \
-      body_node.CalcMassMatrixOffDiagonalBlock##Rnv(                     \
-          R_start_in_v, H_PB_W_cache, Fm_CBo_W, M);                      \
+      body_node.CalcMassMatrixOffDiagonalBlock##Bnv(                     \
+          B_start_in_v, H_PB_W_cache, Fm_CPo_W, M);                      \
     }                                                                    \
   }
 
diff --git a/multibody/tree/body_node_impl.h b/multibody/tree/body_node_impl.h
@@ -92,12 +92,17 @@ class BodyNodeImpl final : public BodyNode<T> {
       const T* positions, const PositionKinematicsCacheInM<T>& pcm,
       const T* velocities, VelocityKinematicsCacheInM<T>* vcm) const final;
 
-  void CalcMassMatrixContribution_TipToBase(
+  void CalcMassMatrixContributionInWorld_TipToBase(
       const PositionKinematicsCache<T>& pc,
-      const std::vector<SpatialInertia<T>>& Mc_B_W_cache,
+      const std::vector<SpatialInertia<T>>& I_BBo_W_cache,
       const std::vector<Vector6<T>>& H_PB_W_cache,
       EigenPtr<MatrixX<T>> M) const final;
 
+  void CalcMassMatrixContributionInM_TipToBase(
+      const PositionKinematicsCacheInM<T>& pcm,
+      const std::vector<SpatialInertia<T>>& I_BMo_M_cache,
+      EigenPtr<MatrixX<T>> M) const final;
+
   // Declare functions for the six sizes of mass matrix off-diagonal
   // blocks.
 #define DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(Rnv)                     \
diff --git a/multibody/tree/body_node_impl_mass_matrix.cc b/multibody/tree/body_node_impl_mass_matrix.cc
@@ -24,110 +24,108 @@ namespace multibody {
 namespace internal {
 
 template <typename T, class ConcreteMobilizer>
-void BodyNodeImpl<T, ConcreteMobilizer>::CalcMassMatrixContribution_TipToBase(
-    const PositionKinematicsCache<T>& pc,
-    const std::vector<SpatialInertia<T>>& Mc_B_W_cache,
-    const std::vector<Vector6<T>>& H_PB_W_cache, EigenPtr<MatrixX<T>> M) const {
+void BodyNodeImpl<T, ConcreteMobilizer>::
+    CalcMassMatrixContributionInWorld_TipToBase(
+        const PositionKinematicsCache<T>& pc,
+        const std::vector<SpatialInertia<T>>& I_BBo_W_cache,
+        const std::vector<Vector6<T>>& H_PB_W_cache,
+        EigenPtr<MatrixX<T>> M) const {
   // Welds only contribute to composite mass properties. Their effect on the
   // mass matrix gets included when we get to the composite's non-weld inboard
   // mobilizer.
   if constexpr (kNv != 0) {
     // This node's 6x6 composite body inertia.
-    const SpatialInertia<T>& Mc_C_W = Mc_B_W_cache[mobod_index()];
+    const SpatialInertia<T>& I_BBo_W = I_BBo_W_cache[mobod_index()];
 
     // Across-mobilizer 6 x kNv hinge matrix, from C's parent Cp to C.
-    const auto H_CpC_W = get_H(H_PB_W_cache);  // 6 x kNv fixed size Map.
+    const auto H_PB_W = get_H(H_PB_W_cache);  // 6 x kNv fixed size Map.
 
     // The composite body algorithm considers the system at rest, when
-    // generalized velocities are zero.
+    // generalized velocities are zero. Here m=kNv, this body's mobilizer's
+    // number of dofs.
     // Now if we consider this node's generalized accelerations as the matrix
-    // vm_dot = Iₘ, the identity matrix in ℝᵐˣᵐ, the spatial acceleration A_WC
+    // vm_dot = Iₘ, the identity matrix in ℝᵐˣᵐ, the spatial acceleration A_WB
     // is in ℝ⁶ˣᵐ. That is, we are considering each case in which all
     // generalized accelerations are zero but the m-th generalized
     // acceleration for this node equals one.
     // This node's spatial acceleration can be written as:
-    //   A_WC = Φᵀ(p_CpC) * A_WCp + Ac_WC + Ab_CpC_W + H_CpC_W * vm_dot
-    // where A_WCp is the spatial acceleration of the parent node's body Cp,
-    // Ac_WC include the centrifugal and Coriolis terms, and Ab_CpC_W is the
-    // spatial acceleration bias of the hinge Jacobian matrix H_CpC_W.
+    //   A_WB = Φᵀ(p_PB) * A_WP + A_WB + Ab_PB_W + H_PB_W * vm_dot
+    // where A_WP is the spatial acceleration of the parent node's body P,
+    // A_WB includes the centrifugal and Coriolis terms, and Ab_PC_W is the
+    // spatial acceleration bias of the hinge Jacobian matrix H_PB_W.
     // Now, since all generalized accelerations but vm_dot are zero, then
-    // A_WCp is zero.  Since the system is at rest, Ac_WC and Ab_CpC_W are
-    // zero.
-    // Therefore, for vm_dot = Iₘ, we have that A_WC = H_CpC_W.
-    const auto& A_WC = H_CpC_W;  // 6 x kNv, fixed-size Map.
+    // A_WP is zero.  Since the system is at rest, A_WB and Ab_PB_W are also
+    // zero. Therefore, for vm_dot = Iₘ, we have that A_WB = H_PB_W.
+    const auto& A_WB = H_PB_W;  // 6 x kNv, fixed-size Map.
 
     // If we consider the closed system composed of the composite body held by
     // its mobilizer, the Newton-Euler equations state:
-    //   Fm_CCo_W = Mc_C_W * A_WC + Fb_C_W
-    // where Fm_CCo_W is the spatial force at this node's mobilizer.
-    // Since the system is at rest, we have Fb_C_W = 0 and thus:
-    // Done as a general matrix multiply, this is 66 * kNv flops.
-    // TODO(sherm1) do better!
-    const Eigen::Matrix<T, 6, kNv> Fm_CCo_W = Mc_C_W * A_WC;
+    //   Fm_BBo_W = I_BBo_W * A_WB + Fb_BBo_W
+    // where Fm_BBo_W is the spatial force at this node's mobilizer.
+    // Since the system is at rest, we have Fb_BBo_W = 0 and thus:
+    const Eigen::Matrix<T, 6, kNv> Fm_BBo_W = I_BBo_W * A_WB;  // expensive
 
-    const int composite_start_in_v = mobilizer().velocity_start_in_v();
+    const int B_start_in_v = mobilizer().velocity_start_in_v();
 
     // Diagonal block corresponding to current node (mobod_index).
-    M->template block<kNv, kNv>(composite_start_in_v, composite_start_in_v) =
-        H_CpC_W.transpose() * Fm_CCo_W;  // kNv * kNv * 11 flops
+    M->template block<kNv, kNv>(B_start_in_v, B_start_in_v) =
+        H_PB_W.transpose() * Fm_BBo_W;  // kNv * kNv * 11 flops
 
-    // We recurse the tree inwards from C all the way to the root. We define
+    // We recurse the tree inwards from B all the way to the root. We define
     // the frames:
-    //  - B:  the frame for the current node, body_node.
-    //  - Bc: B's child node frame, child_node.
-    //  - P:  B's parent node frame.
-    const BodyNode<T>* child_node = this;  // Child starts at frame C.
-    const BodyNode<T>* body_node = this->parent_body_node();  // Inboard body.
-    Eigen::Matrix<T, 6, kNv> Fm_CBo_W = Fm_CCo_W;
-
-    while (body_node->mobod_index() != world_mobod_index()) {
-      const Vector3<T>& p_BoBc_W = pc.get_p_PoBo_W(child_node->mobod_index());
-      // In place rigid shift of the spatial force in each column of
-      // Fm_CBo_W, from Bc to Bo. Before this computation, Fm_CBo_W actually
-      // stores Fm_CBc_W from the previous recursion. At the end of this
-      // computation, Fm_CBo_W stores the spatial force on composite body C,
-      // shifted to Bo, and expressed in the world W. That is, we are doing
-      // Fm_CBo_W = Fm_CBc_W.Shift(p_BcB_W).
-
-      // This is SpatialForce<T>::ShiftInPlace(&Fm_CBo_W, -p_BoBc_W) but
+    //  - C: child composite body, initially B
+    //  - P: parent composite body, initially B's inboard body
+    const BodyNode<T>* child_node = this;
+    const BodyNode<T>* parent_node = this->parent_body_node();
+    Eigen::Matrix<T, 6, kNv> Fm_CPo_W = Fm_BBo_W;  // Initially Fm_CCo_W.
+
+    while (parent_node->mobod_index() != world_mobod_index()) {
+      const Vector3<T>& p_PC_W = pc.get_p_PoBo_W(child_node->mobod_index());
+
+      // In place rigid shift of the spatial force in each column of Fm_CCo_W,
+      // from Co to Po. Before this computation, Fm_CPo_W actually stores
+      // Fm_CCo_W from the previous recursion. At the end of this computation,
+      // Fm_CPo_W stores the spatial force on child composite body C, shifted to
+      // parent origin Po, and expressed in the world W. That is, we are doing
+      // Fm_CPo_W = Fm_CCo_W.Shift(p_CoPo_W).
+
+      // This is SpatialForce<T>::ShiftInPlace(&Fm_CCo_W, -p_PoCo_W) but
       // done with fixed sizes (no loop if kNv==1). 12 * kNv flops
-      // TODO(sherm1) Consider moving this to a templatized ShiftInPlace
-      //  API in SpatialForce (and other spatial vector classes?).
       for (int col = 0; col < kNv; ++col) {
         // Ugly Eigen intermediate types; don't look!
-        auto torque = Fm_CBo_W.template block<3, 1>(0, col);
-        const auto force = Fm_CBo_W.template block<3, 1>(3, col);
-        torque += p_BoBc_W.cross(force);  // + because we're negating p_BoBc
+        auto torque = Fm_CPo_W.template block<3, 1>(0, col);
+        const auto force = Fm_CPo_W.template block<3, 1>(3, col);
+        torque += p_PC_W.cross(force);  // + because we're negating p_PC
       }
 
       CalcMassMatrixOffDiagonalDispatcher<T, kNv>::Dispatch(
-          *body_node, composite_start_in_v, H_PB_W_cache, Fm_CBo_W, M);
+          *parent_node, B_start_in_v, H_PB_W_cache, Fm_CPo_W, M);
 
-      child_node = body_node;                      // Update child node Bc.
-      body_node = child_node->parent_body_node();  // Update node B.
+      child_node = parent_node;                      // Update child node C.
+      parent_node = child_node->parent_body_node();  // Update parent node P.
     }
   }
 }
 
-// This is the inner loop of the CalcMassMatrix() algorithm. Rnv is the
-// size of the outer-loop body node R's mobilizer, kNv is the size of the
-// current body B's ConcreteMobilizer encountered on R's inboard sweep.
-// Cost is Rnv*kNv*11 flops.
-#define DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(Rnv)                             \
+// This is the inner loop of the CalcMassMatrix() algorithm. Bnv is the
+// size of the outer-loop body node B's mobilizer, kNv is the size of the
+// current inboard body P's ConcreteMobilizer encountered on B's inboard sweep.
+// Cost is Bnv*kNv*11 flops.
+#define DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(Bnv)                             \
   template <typename T, class ConcreteMobilizer>                               \
   void                                                                         \
-      BodyNodeImpl<T, ConcreteMobilizer>::CalcMassMatrixOffDiagonalBlock##Rnv( \
-          int R_start_in_v, const std::vector<Vector6<T>>& H_PB_W_cache,       \
-          const Eigen::Matrix<T, 6, Rnv>& Fm_CBo_W, EigenPtr<MatrixX<T>> M)    \
+      BodyNodeImpl<T, ConcreteMobilizer>::CalcMassMatrixOffDiagonalBlock##Bnv( \
+          int B_start_in_v, const std::vector<Vector6<T>>& H_PB_W_cache,       \
+          const Eigen::Matrix<T, 6, Bnv>& Fm_CPo_W, EigenPtr<MatrixX<T>> M)    \
           const {                                                              \
     if constexpr (kNv != 0) {                                                  \
-      const auto H_PB_W = get_H(H_PB_W_cache); /* 6 x kNv fixed-size Map */    \
-      const Eigen::Matrix<T, kNv, Rnv> HtFm = H_PB_W.transpose() * Fm_CBo_W;   \
-      const int body_start_in_v = mobilizer().velocity_start_in_v();           \
+      const auto H_PC_W = get_H(H_PB_W_cache); /* 6 x kNv fixed-size Map */    \
+      const Eigen::Matrix<T, kNv, Bnv> HtFm = H_PC_W.transpose() * Fm_CPo_W;   \
+      const int P_start_in_v = mobilizer().velocity_start_in_v();              \
       /* Update the appropriate block and its symmetric partner. */            \
-      auto block = M->template block<kNv, Rnv>(body_start_in_v, R_start_in_v); \
+      auto block = M->template block<kNv, Bnv>(P_start_in_v, B_start_in_v);    \
       block = HtFm;                                                            \
-      M->template block<Rnv, kNv>(R_start_in_v, body_start_in_v) =             \
+      M->template block<Bnv, kNv>(B_start_in_v, P_start_in_v) =                \
           block.transpose();                                                   \
     }                                                                          \
   }
@@ -141,6 +139,52 @@ DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(6)
 
 #undef DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK
 
+template <typename T, class ConcreteMobilizer>
+void BodyNodeImpl<T, ConcreteMobilizer>::
+    CalcMassMatrixContributionInM_TipToBase(
+        const PositionKinematicsCacheInM<T>& pcm,
+        const std::vector<SpatialInertia<T>>& I_BMo_M_cache,
+        EigenPtr<MatrixX<T>> M) const {
+  // TODO(sherm1) Write this & corresponding off-diagonal.
+  unused(pcm, I_BMo_M_cache, M);
+}
+
+// This is the inner loop of the CalcMassMatrix() algorithm. Bnv is the
+// size of the outer-loop body node B's mobilizer, kNv is the size of the
+// current inboard body P's ConcreteMobilizer encountered on B's inboard sweep.
+// C is the outboard (child) body whose inboard body was P.
+// Cost depends on the force projection inline for P's mobilizer (very cheap
+// for 1-dof mobilizers).
+#define DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M(Bnv)                        \
+  template <typename T, class ConcreteMobilizer>                               \
+  void                                                                         \
+      BodyNodeImpl<T, ConcreteMobilizer>::CalcMassMatrixOffDiagonalBlock##Bnv( \
+          int B_start_in_v, const Eigen::Matrix<T, 6, Bnv>& Fm_CMp_Mp,         \
+          EigenPtr<MatrixX<T>> M)  const {                                     \
+    if constexpr (kNv != 0) {                                                  \
+      const Eigen::Matrix<T, kNv, Bnv> HtFm;                                   \
+      for (int j=0; j < Bnv; ++j) {                                            \
+        /* Calculates tau = H_FM_Mᵀ * F_PMp_M */                               \
+        mobilizer_->calc_tau_from_M(X_FM, q, Fm_CMp_Mp.col(j), &HtFm.col(j));  \
+      }                                                                        \
+      const int P_start_in_v = mobilizer().velocity_start_in_v();              \
+      /* Update the appropriate block and its symmetric partner. */            \
+      auto block = M->template block<kNv, Bnv>(P_start_in_v, B_start_in_v);    \
+      block = HtFm;                                                            \
+      M->template block<Bnv, kNv>(B_start_in_v, P_start_in_v) =                \
+          block.transpose();                                                   \
+    }                                                                          \
+  }
+
+DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M(1)
+DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M(2)
+DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M(3)
+DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M(4)
+DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M(5)
+DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M(6)
+
+#undef DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M
+
 // Macro used to explicitly instantiate implementations for every mobilizer.
 #define EXPLICITLY_INSTANTIATE_IMPLS(T)                           \
   template class BodyNodeImpl<T, CurvilinearMobilizer<T>>;        \
diff --git a/multibody/tree/body_node_world.h b/multibody/tree/body_node_world.h
@@ -56,12 +56,18 @@ class BodyNodeWorld final : public BodyNode<T> {
     DRAKE_UNREACHABLE();
   }
 
-  void CalcMassMatrixContribution_TipToBase(
+  void CalcMassMatrixContributionInWorld_TipToBase(
       const PositionKinematicsCache<T>&, const std::vector<SpatialInertia<T>>&,
       const std::vector<Vector6<T>>&, EigenPtr<MatrixX<T>>) const final {
     DRAKE_UNREACHABLE();
   }
 
+  void CalcMassMatrixContributionInM_TipToBase(
+      const PositionKinematicsCacheInM<T>&,
+      const std::vector<SpatialInertia<T>>&, EigenPtr<MatrixX<T>>) const final {
+    DRAKE_UNREACHABLE();
+  }
+
 #define DEFINE_DUMMY_OFF_DIAGONAL_BLOCK(Rnv)                                \
   void CalcMassMatrixOffDiagonalBlock##Rnv(                                 \
       int, const std::vector<Vector6<T>>&, const Eigen::Matrix<T, 6, Rnv>&, \
diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc
diff --git a/multibody/tree/multibody_tree.h b/multibody/tree/multibody_tree.h
diff --git a/multibody/tree/test/body_node_test.cc b/multibody/tree/test/body_node_test.cc

Original file line number	Diff line number	Diff line change
`@@ -85,10 +85,10 @@ void BodyNode<T>::CalcCompositeBodyInertiaInM_TipToBase(`
`85`	`85`	`const math::RigidTransform<T>& X_MbMc = pcm.get_X_MpM(child_node_index);`
`86`	`86`	`// Shift the child's composite body inertia to B's Mb frame origin Mbo, but`
`87`	`87`	`// still expressed in Mc (so Mx==Mc here).`
`88`		`- const Vector3<T> p_McMb_Mb = -X_MbMc.translation(); // 3 flops`
	`88`	`+ const Vector3<T> p_McMb_Mb = -X_MbMc.translation(); // 3 flops`
`89`	`89`	`SpatialInertia<T> I_CMb_Mx = I_CMc_Mc.Shift(p_McMb_Mb); // 37 flops`
`90`	`90`	`I_CMb_Mx.ReExpressInPlace(X_MbMc.rotation()); // Now Mx==Mb, 72 flops`
`91`		`- I_BMb_Mb += I_CMb_Mx; // 36 flops`
	`91`	`+ I_BMb_Mb += I_CMb_Mx; // 36 flops`
`92`	`92`	`}`
`93`	`93`	`}`
`94`	`94`