Cache body spatial inertia

Author: sherm1

multibody/tree/frame_body_pose_cache.h

@@ -40,6 +40,7 @@ class FrameBodyPoseCache {
       : X_BF_pool_(num_frame_body_pose_slots_needed),
         X_FB_pool_(num_frame_body_pose_slots_needed),
         is_X_BF_identity_(num_frame_body_pose_slots_needed),
+        M_BBo_B_pool_(num_mobods, SpatialInertia<T>::NaN()),
         X_MbF_pool_(num_frames),
         X_FMb_pool_(num_frames),
         is_X_MbF_identity_(num_frames),
@@ -74,6 +75,12 @@ class FrameBodyPoseCache {
     return static_cast<bool>(is_X_BF_identity_[body_pose_index]);
   }
 
+  const SpatialInertia<T>& get_M_BBo_B(MobodIndex index) const {
+    // This method must be very fast in Release.
+    DRAKE_ASSERT(0 <= index && index < ssize(M_BBo_B_pool_));
+    return M_BBo_B_pool_[index];
+  }
+
   void SetX_BF(int body_pose_index, const math::RigidTransform<T>& X_BF) {
     // This method is only called when parameters change.
     DRAKE_DEMAND(0 <= body_pose_index && body_pose_index < ssize(X_BF_pool_));
@@ -88,6 +95,14 @@ class FrameBodyPoseCache {
     }
   }
 
+  void SetM_BBo_B(MobodIndex index, const SpatialInertia<T>& M_BBo_B) {
+    // This method is only called when parameters change.
+    DRAKE_DEMAND(0 <= index && index < ssize(M_BBo_B_pool_));
+    M_BBo_B_pool_[index] = M_BBo_B;
+  }
+
+  // M-frame quantities below.
+
   const math::RigidTransform<T>& get_X_MbF(FrameIndex index) const {
     // This method must be very fast in Release.
     DRAKE_ASSERT(0 <= index && index < ssize(X_MbF_pool_));
@@ -140,6 +155,10 @@ class FrameBodyPoseCache {
   std::vector<math::RigidTransform<T>> X_FB_pool_;
   std::vector<uint8_t> is_X_BF_identity_;  // fast vector<bool> equivalent
 
+  // Spatial inertia of mobilized body B, about its body origin Bo, expressed
+  // in B. These are indexed by MobodIndex.
+  std::vector<SpatialInertia<T>> M_BBo_B_pool_;
+
   // These are indexed by Frame::index().
   std::vector<math::RigidTransform<T>> X_MbF_pool_;
   std::vector<math::RigidTransform<T>> X_FMb_pool_;

multibody/tree/multibody_tree.cc

@@ -1424,6 +1424,8 @@ void MultibodyTree<T>::CalcSpatialInertiasInWorld(
   DRAKE_THROW_UNLESS(M_B_W_all != nullptr);
   DRAKE_THROW_UNLESS(ssize(*M_B_W_all) == topology_.num_mobods());
 
+  const FrameBodyPoseCache<T>& frame_body_pose_cache =
+      EvalFrameBodyPoses(context);
   const PositionKinematicsCache<T>& pc = this->EvalPositionKinematics(context);
 
   // Skip the world.
@@ -1437,8 +1439,8 @@ void MultibodyTree<T>::CalcSpatialInertiasInWorld(
     const RotationMatrix<T>& R_WB = X_WB.rotation();
 
     // Spatial inertia of body B about Bo and expressed in the body frame B.
-    // This call has zero cost for rigid bodies.
-    const SpatialInertia<T> M_B = body.CalcSpatialInertiaInBodyFrame(context);
+    const SpatialInertia<T>& M_B =
+        frame_body_pose_cache.get_M_BBo_B(body.mobod_index());
     // Re-express body B's spatial inertia in the world frame W.
     SpatialInertia<T>& M_B_W = (*M_B_W_all)[body.mobod_index()];
     M_B_W = M_B.ReExpress(R_WB);
@@ -1547,6 +1549,7 @@ void MultibodyTree<T>::CalcFrameBodyPoses(
     const RigidBody<T>& body = mobilizer.outboard_body();
     const SpatialInertia<T> M_BBo_B =
         body.CalcSpatialInertiaInBodyFrame(context);
+    frame_body_poses->SetM_BBo_B(mobod.index(), M_BBo_B);
 
     if (!M_BBo_B.IsNaN()) {
       // Shift and re-express.