Caches H_PB_W. Enables caching for simple_gripper.cc (RobotLocomotion…

…#9736)

examples/simple_gripper/simple_gripper.cc

@@ -311,6 +311,7 @@ int do_main() {
   // Create a context for this system:
   std::unique_ptr<systems::Context<double>> diagram_context =
       diagram->CreateDefaultContext();
+  diagram_context->EnableCaching();
   diagram->SetDefaultContext(diagram_context.get());
   systems::Context<double>& plant_context =
       diagram->GetMutableSubsystemContext(plant, diagram_context.get());

multibody/multibody_tree/multibody_tree.cc

@@ -449,9 +449,8 @@ void MultibodyTree<T>::CalcVelocityKinematicsCache(
   // TODO(amcastro-tri): Loop over bodies to compute velocity kinematics updates
   // corresponding to flexible bodies.
 
-  // TODO(amcastro-tri): Eval H_PB_W from the cache.
-  std::vector<Vector6<T>> H_PB_W_cache(num_velocities());
-  CalcAcrossNodeGeometricJacobianExpressedInWorld(context, pc, &H_PB_W_cache);
+  const std::vector<Vector6<T>>& H_PB_W_cache =
+      tree_system_->EvalAcrossNodeGeometricJacobianExpressedInWorld(context);
 
   // Performs a base-to-tip recursion computing body velocities.
   // This skips the world, depth = 0.
@@ -997,9 +996,8 @@ void MultibodyTree<T>::CalcPointsGeometricJacobianExpressedInWorld(
 
   const PositionKinematicsCache<T>& pc = EvalPositionKinematics(context);
 
-  // TODO(amcastro-tri): Eval H_PB_W from the cache.
-  std::vector<Vector6<T>> H_PB_W_cache(num_velocities());
-  CalcAcrossNodeGeometricJacobianExpressedInWorld(context, pc, &H_PB_W_cache);
+  const std::vector<Vector6<T>>& H_PB_W_cache =
+      tree_system_->EvalAcrossNodeGeometricJacobianExpressedInWorld(context);
 
   // Performs a scan of all bodies in the kinematic path from body_B to the
   // world computing each node's contribution to Jv_WQi.
@@ -1066,9 +1064,8 @@ void MultibodyTree<T>::CalcFrameGeometricJacobianExpressedInWorld(
 
   const PositionKinematicsCache<T>& pc = EvalPositionKinematics(context);
 
-  // TODO(amcastro-tri): Eval H_PB_W from the cache.
-  std::vector<Vector6<T>> H_PB_W_cache(num_velocities());
-  CalcAcrossNodeGeometricJacobianExpressedInWorld(context, pc, &H_PB_W_cache);
+  const std::vector<Vector6<T>>& H_PB_W_cache =
+      tree_system_->EvalAcrossNodeGeometricJacobianExpressedInWorld(context);
 
   // Compute the position of Fq's origin Q in the world frame.
   Vector3<T> p_WoQ_W;
@@ -1251,9 +1248,8 @@ void MultibodyTree<T>::CalcArticulatedBodyInertiaCache(
   const auto& mbt_context =
       dynamic_cast<const MultibodyTreeContext<T>&>(context);
 
-  // TODO(bobbyluig): Eval H_PB_W from the cache.
-  std::vector<Vector6<T>> H_PB_W_cache(num_velocities());
-  CalcAcrossNodeGeometricJacobianExpressedInWorld(context, pc, &H_PB_W_cache);
+  const std::vector<Vector6<T>>& H_PB_W_cache =
+      tree_system_->EvalAcrossNodeGeometricJacobianExpressedInWorld(context);
 
   // Perform tip-to-base recursion, skipping the world.
   for (int depth = tree_height() - 1; depth > 0; depth--) {

multibody/multibody_tree/multibody_tree.h

@@ -2676,6 +2676,20 @@ class MultibodyTree {
     tree_system_ = tree_system;
   }
 
+  /// (Internal) Computes the cache entry associated with the geometric Jacobian
+  /// H_PB_W for each node.
+  /// The geometric Jacobian `H_PB_W` relates to the spatial velocity of B in P
+  /// by `V_PB_W = H_PB_W(q)⋅v_B`, where `v_B` corresponds to the generalized
+  /// velocities associated to body B. `H_PB_W` has size `6 x nm` with `nm` the
+  /// number of mobilities associated with body B.
+  /// `H_PB_W_cache` stores the Jacobian matrices for all nodes in the tree as a
+  /// vector of the columns of these matrices. Therefore `H_PB_W_cache` has as
+  /// many entries as number of generalized velocities in the tree.
+  void CalcAcrossNodeGeometricJacobianExpressedInWorld(
+      const systems::Context<T>& context,
+      const PositionKinematicsCache<T>& pc,
+      std::vector<Vector6<T>>* H_PB_W_cache) const;
+
  private:
   // Make MultibodyTree templated on every other scalar type a friend of
   // MultibodyTree<T> so that CloneToScalar<ToAnyOtherScalar>() can access
@@ -2729,20 +2743,6 @@ class MultibodyTree {
   // that the error message can include that detail.
   void ThrowIfNotFinalized(const char* source_method) const;
 
-  // Computes the cache entry associated with the geometric Jacobian H_PB_W for
-  // each node.
-  // The geometric Jacobian `H_PB_W` relates to the spatial velocity of B in P
-  // by `V_PB_W = H_PB_W(q)⋅v_B`, where `v_B` corresponds to the generalized
-  // velocities associated to body B. `H_PB_W` has size `6 x nm` with `nm` the
-  // number of mobilities associated with body B.
-  // `H_PB_W_cache` stores the Jacobian matrices for all nodes in the tree as a
-  // vector of the columns of these matrices. Therefore `H_PB_W_cache` has as
-  // many entries as number of generalized velocities in the tree.
-  void CalcAcrossNodeGeometricJacobianExpressedInWorld(
-      const systems::Context<T>& context,
-      const PositionKinematicsCache<T>& pc,
-      std::vector<Vector6<T>>* H_PB_W_cache) const;
-
   // Implementation for CalcMassMatrixViaInverseDynamics().
   // It assumes:
   //  - The position kinematics cache object is already updated to be in sync

multibody/multibody_tree/multibody_tree_system.cc

@@ -2,6 +2,7 @@
 
 #include <memory>
 #include <utility>
+#include <vector>
 
 #include "drake/common/autodiff.h"
 #include "drake/common/default_scalars.h"
@@ -124,6 +125,25 @@ void MultibodyTreeSystem<T>::Finalize() {
   velocity_kinematics_cache_index_ =
       velocity_kinematics_cache_entry.cache_index();
 
+  // Declare cache entry for H_PB_W(q).
+  // The type of this cache value is std::vector<Vector6<T>>.
+  auto& H_PB_W_cache_entry = this->DeclareCacheEntry(
+      std::string("H_PB_W(q)"),
+      [tree = tree_.get()]() {
+        return systems::AbstractValue::Make(
+            std::vector<Vector6<T>>(tree->num_velocities()));
+      },
+      [tree = tree_.get()](const systems::ContextBase& context_base,
+                           systems::AbstractValue* cache_value) {
+        auto& context = dynamic_cast<const Context<T>&>(context_base);
+        auto& H_PB_W_cache =
+            cache_value->GetMutableValue<std::vector<Vector6<T>>>();
+        tree->CalcAcrossNodeGeometricJacobianExpressedInWorld(
+            context, tree->EvalPositionKinematics(context), &H_PB_W_cache);
+      },
+      {this->cache_entry_ticket(position_kinematics_cache_index_)});
+  H_PB_W_cache_index_ = H_PB_W_cache_entry.cache_index();
+
   // TODO(sherm1) Allocate articulated body inertia cache.
 
   already_finalized_ = true;

multibody/multibody_tree/multibody_tree_system.h

@@ -3,7 +3,9 @@
 #include <memory>
 #include <string>
 #include <utility>
+#include <vector>
 
+#include "drake/common/eigen_types.h"
 #include "drake/multibody/multibody_tree/position_kinematics_cache.h"
 #include "drake/multibody/multibody_tree/velocity_kinematics_cache.h"
 #include "drake/systems/framework/cache_entry.h"
@@ -96,6 +98,26 @@ class MultibodyTreeSystem : public systems::LeafSystem<T> {
         .template Eval<VelocityKinematicsCache<T>>(context);
   }
 
+  /** Returns a reference to the up to date cached value for the
+  across-mobilizer geometric Jacobian H_PB_W in the given Context, recalculating
+  it first if necessary. Also if necessary, the PositionKinematicsCache will be
+  recalculated as well (since it stores H_FM(q) for each mobilizer and X_WB(q)
+  for each body).
+  The geometric Jacobian `H_PB_W` relates to the spatial velocity of B in P
+  by `V_PB_W = H_PB_W(q)⋅v_B`, where `v_B` corresponds to the generalized
+  velocities associated to body B. `H_PB_W` has size `6 x nm` with `nm` the
+  number of mobilities associated with body B.
+  The returned `std::vector` stores the Jacobian matrices for all nodes in the
+  tree  as a vector of the columns of these matrices. Therefore
+  the returned `std::vector` of columns  has as many entries as number of
+  generalized velocities in the tree. */
+  const std::vector<Vector6<T>>&
+  EvalAcrossNodeGeometricJacobianExpressedInWorld(
+      const systems::Context<T> &context) const {
+    return this->get_cache_entry(H_PB_W_cache_index_)
+        .template Eval<std::vector<Vector6<T>>>(context);
+  }
+
   // TODO(sherm1) Add ArticulatedBodyInertiaCache.
 
  protected:
@@ -169,6 +191,7 @@ class MultibodyTreeSystem : public systems::LeafSystem<T> {
   std::unique_ptr<drake::multibody::MultibodyTree<T>> tree_;
   systems::CacheIndex position_kinematics_cache_index_;
   systems::CacheIndex velocity_kinematics_cache_index_;
+  systems::CacheIndex H_PB_W_cache_index_;
 
   // Used to enforce "finalize once" restriction for protected-API users.
   bool already_finalized_{false};