Nuke useless FrameBase class; adds frame body pose cache.

Convert BodyNode functions to use the new cache.

Author: sherm1

multibody/tree/BUILD.bazel

@@ -59,13 +59,15 @@ drake_cc_library(
         "acceleration_kinematics_cache.cc",
         "articulated_body_force_cache.cc",
         "articulated_body_inertia_cache.cc",
+        "frame_body_pose_cache.cc",
         "position_kinematics_cache.cc",
         "velocity_kinematics_cache.cc",
     ],
     hdrs = [
         "acceleration_kinematics_cache.h",
         "articulated_body_force_cache.h",
         "articulated_body_inertia_cache.h",
+        "frame_body_pose_cache.h",
         "position_kinematics_cache.h",
         "velocity_kinematics_cache.h",
     ],
@@ -89,7 +91,6 @@ drake_cc_library(
         "fixed_offset_frame.cc",
         "force_element.cc",
         "frame.cc",
-        "frame_base.cc",
         "joint.cc",
         "joint_actuator.cc",
         "linear_bushing_roll_pitch_yaw.cc",
@@ -133,7 +134,6 @@ drake_cc_library(
         "fixed_offset_frame.h",
         "force_element.h",
         "frame.h",
-        "frame_base.h",
         "joint.h",
         "joint_actuator.h",
         "linear_bushing_roll_pitch_yaw.h",

multibody/tree/body_node.h

@@ -187,6 +187,7 @@ class BodyNode : public MultibodyElement<T> {
   // in the tree.)
   void CalcPositionKinematicsCache_BaseToTip(
       const systems::Context<T>& context,
+      const FrameBodyPoseCache<T>& frame_body_pose_cache,
       PositionKinematicsCache<T>* pc) const {
     // This method must not be called for the "world" body node.
     DRAKE_ASSERT(topology_.rigid_body != world_index());
@@ -203,7 +204,7 @@ class BodyNode : public MultibodyElement<T> {
     // mobilizer. q(W:P) denotes all generalized positions in the kinematics
     // path between World and the parent body P. It assumes we are in a
     // base-to-tip recursion and therefore `X_WP` has already been updated.
-    CalcAcrossMobilizerBodyPoses_BaseToTip(context, pc);
+    CalcAcrossMobilizerBodyPoses_BaseToTip(frame_body_pose_cache, pc);
 
     // TODO(amcastro-tri):
     // Update Body specific kinematics. These include:
@@ -381,6 +382,7 @@ class BodyNode : public MultibodyElement<T> {
   // MultibodyTree::CalcAccelerationKinematicsCache().
   void CalcSpatialAcceleration_BaseToTip(
       const systems::Context<T>& context,
+      const FrameBodyPoseCache<T>& frame_body_poses_cache,
       const PositionKinematicsCache<T>& pc,
       const VelocityKinematicsCache<T>* vc,
       const VectorX<T>& mbt_vdot,
@@ -456,12 +458,11 @@ class BodyNode : public MultibodyElement<T> {
     // =========================================================================
     // Computation of A_PB = DtP(V_PB), Eq. (4).
 
-    // TODO(amcastro-tri): consider caching these. Also used in velocity
-    //  kinematics.
-    const math::RotationMatrix<T> R_PF =
-        frame_F.CalcRotationMatrixInBodyFrame(context);
-    const math::RigidTransform<T> X_MB =
-        frame_M.CalcPoseInBodyFrame(context).inverse();
+    const math::RigidTransform<T>& X_PF =
+        frame_F.get_X_BF(frame_body_poses_cache);
+    const math::RotationMatrix<T>& R_PF = X_PF.rotation();
+    const math::RigidTransform<T>& X_MB =
+        frame_M.get_X_FB(frame_body_poses_cache);
 
     // Form the rotation matrix relating the world frame W and parent body P.
     // Available since we are called within a base-to-tip recursion.
@@ -595,6 +596,7 @@ class BodyNode : public MultibodyElement<T> {
   // MultibodyTree::CalcInverseDynamics().
   void CalcInverseDynamics_TipToBase(
       const systems::Context<T>& context,
+      const FrameBodyPoseCache<T>& frame_body_pose_cache,
       const PositionKinematicsCache<T>& pc,
       const std::vector<SpatialInertia<T>>& M_B_W_cache,
       const std::vector<SpatialForce<T>>* Fb_Bo_W_cache,
@@ -675,7 +677,8 @@ class BodyNode : public MultibodyElement<T> {
     // Compute shift vector from Bo to Mo expressed in the world frame W.
     const Frame<T>& frame_M = outboard_frame();
     DRAKE_DEMAND(frame_M.body().index() == body_B.index());
-    const math::RigidTransform<T> X_BM = frame_M.CalcPoseInBodyFrame(context);
+    const math::RigidTransform<T>& X_BM =
+        frame_M.get_X_BF(frame_body_pose_cache);  // F==M
     const Vector3<T>& p_BoMo_B = X_BM.translation();
     const math::RigidTransform<T>& X_WB = get_X_WB(pc);
     const math::RotationMatrix<T>& R_WB = X_WB.rotation();
@@ -700,8 +703,8 @@ class BodyNode : public MultibodyElement<T> {
       // p_CoMc_W:
       const Frame<T>& frame_Mc = child_node->outboard_frame();
       const math::RotationMatrix<T>& R_WC = child_node->get_X_WB(pc).rotation();
-      const math::RigidTransform<T> X_CMc =
-          frame_Mc.CalcPoseInBodyFrame(context);
+      const math::RigidTransform<T>& X_CMc =
+          frame_Mc.get_X_BF(frame_body_pose_cache);  // B==C, F==Mc
       const Vector3<T>& p_CoMc_W = R_WC * X_CMc.translation();
 
       // Shift position vector from child C outboard mobilizer frame Mc to body
@@ -775,6 +778,7 @@ class BodyNode : public MultibodyElement<T> {
   // with `context` by MultibodyTree::CalcPositionKinematicsCache().
   void CalcAcrossNodeJacobianWrtVExpressedInWorld(
       const systems::Context<T>& context,
+      const FrameBodyPoseCache<T>& frame_body_pose_cache,
       const PositionKinematicsCache<T>& pc,
       EigenPtr<MatrixX<T>> H_PB_W) const {
     // Checks on the input arguments.
@@ -788,10 +792,11 @@ class BodyNode : public MultibodyElement<T> {
     // Outboard frame M of this node's mobilizer.
     const Frame<T>& frame_M = outboard_frame();
 
-    const math::RotationMatrix<T> R_PF =
-        frame_F.CalcRotationMatrixInBodyFrame(context);
-    const math::RigidTransform<T> X_MB =
-        frame_M.CalcPoseInBodyFrame(context).inverse();
+    const math::RigidTransform<T>& X_PF =
+        frame_F.get_X_BF(frame_body_pose_cache);  // B==P
+    const math::RotationMatrix<T>& R_PF = X_PF.rotation();
+    const math::RigidTransform<T>& X_MB =
+        frame_M.get_X_FB(frame_body_pose_cache);  // F==M
 
     // Form the rotation matrix relating the world frame W and parent body P.
     const math::RotationMatrix<T>& R_WP = get_R_WP(pc);
@@ -1296,10 +1301,12 @@ class BodyNode : public MultibodyElement<T> {
   // @pre pc and vc previously computed to be in sync with `context.
   //
   // @throws when `Ab_WB` is nullptr.
-  void CalcSpatialAccelerationBias(const systems::Context<T>& context,
-                                   const PositionKinematicsCache<T>& pc,
-                                   const VelocityKinematicsCache<T>& vc,
-                                   SpatialAcceleration<T>* Ab_WB) const {
+  void CalcSpatialAccelerationBias(
+      const systems::Context<T>& context,
+      const FrameBodyPoseCache<T>& frame_body_pose_cache,
+      const PositionKinematicsCache<T>& pc,
+      const VelocityKinematicsCache<T>& vc,
+      SpatialAcceleration<T>* Ab_WB) const {
     DRAKE_THROW_UNLESS(Ab_WB != nullptr);
     // As a guideline for developers, please refer to @ref
     // abi_computing_accelerations for a detailed description and derivation of
@@ -1311,10 +1318,11 @@ class BodyNode : public MultibodyElement<T> {
     const Frame<T>& frame_M = outboard_frame();
 
     // Compute R_PF and X_MB.
-    const math::RotationMatrix<T> R_PF =
-        frame_F.CalcRotationMatrixInBodyFrame(context);
-    const math::RigidTransform<T> X_MB =
-        frame_M.CalcPoseInBodyFrame(context).inverse();
+    const math::RigidTransform<T>& X_PF =
+        frame_F.get_X_BF(frame_body_pose_cache);  // B==P
+    const math::RotationMatrix<T>& R_PF = X_PF.rotation();
+    const math::RigidTransform<T>& X_MB =
+        frame_M.get_X_FB(frame_body_pose_cache);  // F==M
 
     // Parent position in the world is available from the position kinematics.
     const math::RotationMatrix<T>& R_WP = get_R_WP(pc);
@@ -1733,7 +1741,7 @@ class BodyNode : public MultibodyElement<T> {
   // between the world and the parent body P. It assumes we are in a base-to-tip
   // recursion and therefore `X_WP` has already been updated.
   void CalcAcrossMobilizerBodyPoses_BaseToTip(
-      const systems::Context<T>& context,
+      const FrameBodyPoseCache<T>& frame_body_pose_cache,
       PositionKinematicsCache<T>* pc) const {
     // RigidBody for this node.
     const RigidBody<T>& body_B = body();
@@ -1753,8 +1761,10 @@ class BodyNode : public MultibodyElement<T> {
     // - X_FM(q_B)
     // - X_WP(q(W:B)), where q(W:B) includes all positions in the kinematics
     //                 path from body B to the world W.
-    const math::RigidTransform<T> X_MB =
-        frame_M.CalcPoseInBodyFrame(context).inverse();
+    const math::RigidTransform<T>& X_MB =
+        frame_M.get_X_FB(frame_body_pose_cache);  // F==M
+    const math::RigidTransform<T>& X_PF =
+        frame_F.get_X_BF(frame_body_pose_cache);  // B==P
     const math::RigidTransform<T>& X_FM =
         get_X_FM(*pc);  // mobilizer.Eval_X_FM(ctx)
     const math::RigidTransform<T>& X_WP =
@@ -1771,12 +1781,7 @@ class BodyNode : public MultibodyElement<T> {
     //  In that case X_FB = X_FM as suggested by setting X_MB = Identity.
     const math::RigidTransform<T> X_FB = X_FM * X_MB;
 
-    // Given the pose X_FB of body frame B measured in the mobilizer inboard
-    // frame F, we can ask frame F (who's parent body is P) for the pose of body
-    // B measured in the frame of the parent body P.
-    // In the particular case F = B, this method directly returns X_FB.
-    X_PB = frame_F.CalcOffsetPoseInBody(context, X_FB);
-
+    X_PB = X_PF * X_FB;
     X_WB = X_WP * X_PB;
 
     // Compute shift vector p_PoBo_W from the parent origin to the body origin.

multibody/tree/frame.h

@@ -3,11 +3,12 @@
 #include <memory>
 #include <optional>
 #include <string>
+#include <vector>
 
 #include "drake/common/autodiff.h"
 #include "drake/common/nice_type_name.h"
-#include "drake/multibody/math/spatial_algebra.h"
-#include "drake/multibody/tree/frame_base.h"
+#include "drake/multibody/tree/frame_body_pose_cache.h"
+#include "drake/multibody/tree/multibody_element.h"
 #include "drake/multibody/tree/multibody_tree_indexes.h"
 #include "drake/multibody/tree/multibody_tree_topology.h"
 #include "drake/multibody/tree/scoped_name.h"
@@ -23,39 +24,41 @@ std::string DeprecateWhenEmptyName(std::string name, std::string_view type);
 template<typename T> class RigidBody;
 
 /// %Frame is an abstract class representing a _material frame_ (also called a
-/// _physical frame_), meaning that the %Frame's origin is a material point of
-/// a Body.
+/// _physical frame_) of its underlying RigidBody. The %Frame's origin is a
+/// material point of its RigidBody, and its axes have fixed directions
+/// in that body. A %Frame's pose (position and orientation) with respect to its
+/// RigidBodyFrame may be parameterized, but is fixed (not time or state
+/// dependent) once parameters have been set.
 ///
-/// An important characteristic of a %Frame is that forces or torques applied
-/// to a %Frame are applied to the %Frame's underlying Body. Force-producing
+/// An important characteristic of a %Frame is that forces or torques applied to
+/// a %Frame are applied to the %Frame's underlying RigidBody. Force-producing
 /// elements like joints, actuators, and constraints usually employ two %Frames,
 /// with one %Frame connected to one body and the other connected to a different
-/// Body. Every %Frame object can report the RigidBody to which it is attached.
-/// Despite its name, %Frame is not the most general frame in Drake
-/// (see FrameBase for more information).
+/// body. Every %Frame F can report the RigidBody B to which it is attached and
+/// its pose X_BF with respect to B's RigidBodyFrame.
 ///
 /// A %Frame's pose in World (or relative to other frames) is always calculated
-/// starting with its pose relative to its underlying %Body's BodyFrame.
+/// starting with its pose relative to its underlying RigidBodyFrame.
 /// Subclasses derived from %Frame differ in how kinematic calculations are
 /// performed. For example, the angular velocity of a FixedOffsetFrame or
-/// BodyFrame is identical to the angular velocity of its underlying body,
+/// RigidBodyFrame is identical to the angular velocity of its underlying body,
 /// whereas the translational velocity of a FixedOffsetFrame differs from that
-/// of a BodyFrame. If a %Frame is associated with a soft body, kinematic
-/// calculations can depend on the soft body's deformation state variables.
+/// of a RigidBodyFrame.
 ///
-/// A %Frame object does _not_ store a Context (where Context means state that
-/// contains the %Frame's current orientation, position, motion, etc.).
-/// Instead, %Frame provides methods for calculating these %Frame-properties
-/// from a Context passed to %Frame methods.
+/// %Frame provides methods for obtaining its current orientation, position,
+/// motion, etc. from a Context passed to those methods.
 ///
 /// @tparam_default_scalar
 template <typename T>
-class Frame : public FrameBase<T> {
+class Frame : public MultibodyElement<T> {
  public:
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Frame);
 
   ~Frame() override;
 
+  /// Returns this %Frame's unique index.
+  FrameIndex index() const { return this->template index_impl<FrameIndex>(); }
+
   /// Returns a const reference to the body associated to this %Frame.
   const RigidBody<T>& body() const {
     return body_;
@@ -80,6 +83,19 @@ class Frame : public FrameBase<T> {
   /// will be empty (the scope name and the element name).
   ScopedName scoped_name() const;
 
+  /// Returns a reference to the body-relative pose X_BF giving the pose of this
+  /// Frame with respect to its body's RigidBodyFrame. This may depend on
+  /// parameters in the Context but not on time or state. The first time this is
+  /// called after a parameter change will precalculate offset poses for all
+  /// %Frames into the Context's cache; subsequent calls on any %Frame are very
+  /// fast.
+  const math::RigidTransform<T>& EvalPoseInBodyFrame(
+      const systems::Context<T>& context) const {
+    const internal::FrameBodyPoseCache<T>& frame_body_poses =
+        this->GetParentTreeSystem().EvalFrameBodyPoses(context);
+    return get_X_BF(frame_body_poses);
+  }
+
   /// Returns the pose `X_BF` of `this` frame F in the body frame B associated
   /// with this frame.
   /// In particular, if `this` **is** the body frame B, this method directly
@@ -495,14 +511,62 @@ class Frame : public FrameBase<T> {
     return DoCloneToScalar(tree_clone);
   }
 
+  /// @name Internal use only
+  /// These functions work directly with the frame body pose cache entry.
+  //@{
+  /// (Internal use only) A %Frame's pose-in-parent X_PF can be parameterized,
+  /// the parent's pose may also be parameterized, and so on. Thus the
+  /// calculation of this frame's pose in its body (X_BF) can be expensive.
+  /// There is a cache entry that holds the calculated X_BF, evaluated
+  /// whenever parameters change. This allows us to grab X_BF as a const
+  /// reference rather than having to extract and reformat parameters, and
+  /// compose with parent and ancestor poses at runtime.
+  ///
+  /// At the time parameters are allocated we assign a slot in the body pose
+  /// cache entry to each %Frame and record its index using this function. (The
+  /// index for a RigidBodyFrame will refer to an identity transform.) Note that
+  /// the body pose index is not necessarily the same as the %Frame index
+  /// because all RigidBodyFrames can share an entry. (Of course if you know you
+  /// are working with a RigidBodyFrame you don't need to ask about its body
+  /// pose!)
+  void set_body_pose_index_in_cache(int body_pose_index) {
+    body_pose_index_in_cache_ = body_pose_index;
+  }
+
+  /// (Internal use only) Retrieve this %Frame's body pose index in the cache.
+  int get_body_pose_index_in_cache() const {
+    return body_pose_index_in_cache_;
+  }
+
+  /// (Internal use only) Given an already up-to-date frame body pose cache,
+  /// extract X_BF for this %Frame from it.
+  /// @note Be sure you have called MultibodyTreeSystem::EvalFrameBodyPoses()
+  ///       since the last parameter change; we can't check here.
+  /// @retval X_BF pose of this frame in its body's frame
+  const math::RigidTransform<T>& get_X_BF(
+      const internal::FrameBodyPoseCache<T>& frame_body_poses) const {
+    return frame_body_poses.get_X_BF(body_pose_index_in_cache_);
+  }
+
+  /// (Internal use only) Given an already up-to-date frame body pose cache,
+  /// extract X_FB (=X_BF⁻¹) for this %Frame from it.
+  /// @note Be sure you have called MultibodyTreeSystem::EvalFrameBodyPoses()
+  ///       since the last parameter change; we can't check here.
+  /// @retval X_FB inverse of this frame's pose in its body's frame
+  const math::RigidTransform<T>& get_X_FB(
+      const internal::FrameBodyPoseCache<T>& frame_body_poses) const {
+    return frame_body_poses.get_X_FB(body_pose_index_in_cache_);
+  }
+  //@}
+
  protected:
   /// Only derived classes can use this constructor. It creates a %Frame
   /// object attached to `body` and puts the frame in the body's model
   /// instance.
   explicit Frame(
       const std::string& name, const RigidBody<T>& body,
       std::optional<ModelInstanceIndex> model_instance = {})
-      : FrameBase<T>(model_instance.value_or(body.model_instance())),
+      : MultibodyElement<T>(model_instance.value_or(body.model_instance())),
         name_(internal::DeprecateWhenEmptyName(name, "Frame")),
         body_(body) {}
 
@@ -586,6 +650,8 @@ class Frame : public FrameBase<T> {
 
   // The internal bookkeeping topology struct used by MultibodyTree.
   internal::FrameTopology topology_;
+
+  int body_pose_index_in_cache_{-1};
 };
 
 }  // namespace multibody