Don't recompute parameters

Author: sherm1

multibody/tree/multibody_tree.cc

@@ -1404,9 +1404,21 @@ 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);
 
+  // Update parameter-dependent quantities in the position cache if they are
+  // not up to date with respect to the FrameBodyPoseCache in the context
+  // (which we just updated). PositionKinematicsCache needs the
+  // FrameBodyPoseCache serial number to make that determination.
+  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();
+  pc->PrecomputeWorldCompositeIfNeeded(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/multibody_tree_system.h

@@ -98,26 +98,8 @@ class MultibodyTreeSystem : public systems::LeafSystem<T> {
   const FrameBodyPoseCache<T>& EvalFrameBodyPoses(
       const systems::Context<T>& context) const {
     this->ValidateContext(context);
-    const FrameBodyPoseCache<T>& frame_body_pose_cache =
-        frame_body_poses_cache_entry().template Eval<FrameBodyPoseCache<T>>(
-            context);
-
-    // We don't know whether we actually had to update the FrameBodyPoseCache,
-    // but if we did the PositionKinematicsCache will be out of date since
-    // it depends on the parameterized data in the FrameBodyPoseCache.
-    // In that case, we'll fill in now any position kinematics that are state
-    // independent so that we don't have to recalculate when q's change.
-    systems::CacheEntryValue& cev =
-        position_kinematics_cache_entry().get_mutable_cache_entry_value(
-            context);
-    if (cev.is_out_of_date()) {
-      PositionKinematicsCache<T>& pc =
-          cev.GetMutableValueOrThrow<PositionKinematicsCache<T>>();
-      pc.PrecomputeWorldComposite(tree_->forest(), frame_body_pose_cache);
-      // This does not make the pc up to date!
-    }
-
-    return frame_body_pose_cache;
+    return frame_body_poses_cache_entry().template Eval<FrameBodyPoseCache<T>>(
+        context);
   }
 
   /* Returns a reference to the up-to-date PositionKinematicsCache in the

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,27 @@ 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.
-  void PrecomputeWorldComposite(
+  // are fixed to World. Do nothing if the serial number hasn't changed.
+  void PrecomputeWorldCompositeIfNeeded(
       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 +213,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_;