Don't recompute parameters

Author: sherm1

multibody/tree/multibody_tree.cc

@@ -1404,9 +1404,25 @@ void MultibodyTree<T>::CalcPositionKinematicsCache(
     const systems::Context<T>& context, PositionKinematicsCache<T>* pc) const {
   DRAKE_DEMAND(pc != nullptr);
 
+  // Ensure parameter-dependent quantities are up to date.
   const FrameBodyPoseCache<T>& frame_body_pose_cache =
       EvalFrameBodyPoses(context);
 
+  // Check whether parameter-dependent quantities in the position cache are
+  // up to date with respect to the FrameBodyPoseCache in the context. If not,
+  // then we need to update the position cache before we can use it.
+
+  const systems::CacheEntryValue& fbpc_value =
+        tree_system_->frame_body_poses_cache_entry().get_cache_entry_value(
+            context);
+  const int64_t fbpc_serial_number = fbpc_value.serial_number();
+
+  if (pc->world_composite_serial_number() != fbpc_serial_number) {
+    // The position cache is not up to date with respect to the frame body pose
+    // cache, so we need to update it before we can use it.
+    pc->PrecomputeWorldComposite(forest(), frame_body_pose_cache, fbpc_serial_number);
+  }
+
   const Eigen::VectorBlock<const VectorX<T>> q_block = get_positions(context);
   const T* q = q_block.data();
 

multibody/tree/position_kinematics_cache.cc

@@ -27,7 +27,7 @@ PositionKinematicsCache<T>::PositionKinematicsCache(
 }
 
 template <typename T>
-void PositionKinematicsCache<T>::PrecomputeWorldComposite(
+void PositionKinematicsCache<T>::ComputeWorldComposite(
     const SpanningForest& forest,
     const FrameBodyPoseCache<T>& frame_body_pose_cache) {
   const SpanningForest::Mobod& world_mobod = forest.mobods(MobodIndex(0));

multibody/tree/position_kinematics_cache.h

@@ -174,15 +174,28 @@ class PositionKinematicsCache {
   // in X_WL for those links once and for all. X_WL₀ (≜ X_WL[link₀]) and
   // X_WB₀ (≜ X_WB[mobod₀]) are identity transforms (set during allocation).
   // Consequently, X_WLᵢ = X_WB₀ * X_B₀Lᵢ = X_B₀Lᵢ for each of the links Lᵢ that
-  // are fixed to World.
+  // are fixed to World. Do nothing if the serial number hasn't changed.
   void PrecomputeWorldComposite(
       const SpanningForest& forest,
-      const FrameBodyPoseCache<T>& frame_body_pose_cache);
+      const FrameBodyPoseCache<T>& frame_body_pose_cache,
+      int64_t frame_body_pose_cache_serial_number) {
+    if (world_composite_serial_number_ == frame_body_pose_cache_serial_number) {
+      return;
+    }
+    ComputeWorldComposite(forest, frame_body_pose_cache);
+    world_composite_serial_number_ = frame_body_pose_cache_serial_number;
+  }
 
  private:
   // Allocates resources for this position kinematics cache.
   void Allocate();
 
+  // Called when we know we have to recompute the parameter-dependent
+  // world composite kinematics.
+  void ComputeWorldComposite(
+      const SpanningForest& forest,
+      const FrameBodyPoseCache<T>& frame_body_pose_cache);
+
   // Helper method to initialize poses to garbage values including NaNs.
   // This allow us to quickly verify some of the values stored in the pools are
   // never used (however we store them anyway to simplify the indexing).
@@ -201,6 +214,12 @@ class PositionKinematicsCache {
   // (including the World link) and possibly some ephemeral links.
   int num_links_{0};
 
+  // The serial number of the last FrameBodyPoseCache that was used to update
+  // the state-independent quantities in this cache (such as the X_WL for links
+  // fixed to World). This is used to determine whether we need to recompute
+  // those quantities when PrecomputeWorldComposite() is called.
+  int64_t world_composite_serial_number_{-1};
+
   // These are indexed by MobodIndex so are in depth-first order.
   std::vector<RigidTransform<T>> X_WB_pool_;
   std::vector<RigidTransform<T>> X_PB_pool_;