[multibody] Avoid Eigen::Block<Eigen::Ref<VectorX>> (#15451)

* [multibody] Avoid Eigen::Block<Eigen::Ref<VectorX>>

Using a pointer to a pointer is extremely prone to use-after-free errors
when the interior pointer changes through the call sequence.  Just one
pointer indirection is plenty to denote what we intend.

multibody/tree/ball_rpy_joint.h

@@ -201,7 +201,7 @@ class BallRpyJoint final : public Joint<T> {
   /// viscous law `τ = -d⋅ω`, with d the damping coefficient (see damping()).
   void DoAddInDamping(const systems::Context<T>& context,
                       MultibodyForces<T>* forces) const override {
-    Eigen::VectorBlock<Eigen::Ref<VectorX<T>>> t_BMo_F =
+    Eigen::Ref<VectorX<T>> t_BMo_F =
         get_mobilizer()->get_mutable_generalized_forces_from_array(
             &forces->mutable_generalized_forces());
     const Vector3<T>& w_FM = get_angular_velocity(context);

multibody/tree/body_node.h

@@ -1599,20 +1599,12 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
   // Returns an Eigen expression of the vector of generalized accelerations
   // for this node's inboard mobilizer from the vector of generalized
   // accelerations for the entire model.
-  Eigen::VectorBlock<const VectorX<T>> get_accelerations(
-      AccelerationKinematicsCache<T>* ac) const {
-    const VectorX<T>& vdot = ac->get_vdot();
-    return get_velocities_from_array(vdot);
-  }
-
-  // Mutable version of get_accelerations_from_array().
-  Eigen::VectorBlock<Eigen::Ref<VectorX<T>>> get_mutable_accelerations(
+  Eigen::Ref<VectorX<T>> get_mutable_accelerations(
       AccelerationKinematicsCache<T>* ac) const {
     VectorX<T>& vdot = ac->get_mutable_vdot();
     return get_mutable_velocities_from_array(&vdot);
   }
 
-
   // =========================================================================
   // ArticulatedBodyInertiaCache Accessors and Mutators.
 
@@ -1750,14 +1742,7 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
   // tree. Useful for the implementation of operator forms where the generalized
   // velocity (or time derivatives of the generalized velocities) is an argument
   // to the operator.
-  Eigen::VectorBlock<const Eigen::Ref<const VectorX<T>>>
-  get_velocities_from_array(const Eigen::Ref<const VectorX<T>>& v) const {
-    return v.segment(topology_.mobilizer_velocities_start_in_v,
-                     topology_.num_mobilizer_velocities);
-  }
-
-  // Mutable version of get_velocities_from_array().
-  Eigen::VectorBlock<Eigen::Ref<VectorX<T>>> get_mutable_velocities_from_array(
+  Eigen::Ref<VectorX<T>> get_mutable_velocities_from_array(
       EigenPtr<VectorX<T>> v) const {
     DRAKE_ASSERT(v != nullptr);
     return v->segment(topology_.mobilizer_velocities_start_in_v,
@@ -1767,14 +1752,7 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
   // Helper to get an Eigen expression of the vector of generalized forces
   // from a vector of generalized forces for the entire parent multibody
   // tree.
-  Eigen::VectorBlock<const VectorX<T>> get_generalized_forces_from_array(
-      const VectorX<T>& tau) const {
-    return get_velocities_from_array(tau);
-  }
-
-  // Mutable version of get_generalized_forces_from_array()
-  Eigen::VectorBlock<Eigen::Ref<VectorX<T>>>
-  get_mutable_generalized_forces_from_array(
+  Eigen::Ref<VectorX<T>> get_mutable_generalized_forces_from_array(
       EigenPtr<VectorX<T>> tau) const {
     DRAKE_ASSERT(tau != nullptr);
     return get_mutable_velocities_from_array(tau);

multibody/tree/mobilizer.h

@@ -513,16 +513,16 @@ class Mobilizer : public MultibodyElement<Mobilizer, T, MobilizerIndex> {
   // for `this` mobilizer from a vector `q_array` of generalized positions for
   // the entire MultibodyTree model.
   // @pre @p q_array is of size MultibodyTree::num_positions().
-  Eigen::VectorBlock<const Eigen::Ref<const VectorX<T>>>
-  get_positions_from_array(const Eigen::Ref<const VectorX<T>>& q_array) const {
+  Eigen::Ref<const VectorX<T>> get_positions_from_array(
+      const Eigen::Ref<const VectorX<T>>& q_array) const {
     DRAKE_DEMAND(
         q_array.size() == this->get_parent_tree().num_positions());
     return q_array.segment(topology_.positions_start,
                            topology_.num_positions);
   }
 
   // Mutable version of get_positions_from_array().
-  Eigen::VectorBlock<Eigen::Ref<VectorX<T>>> get_mutable_positions_from_array(
+  Eigen::Ref<VectorX<T>> get_mutable_positions_from_array(
       EigenPtr<VectorX<T>> q_array) const {
     DRAKE_DEMAND(q_array != nullptr);
     DRAKE_DEMAND(
@@ -535,7 +535,7 @@ class Mobilizer : public MultibodyElement<Mobilizer, T, MobilizerIndex> {
   // for `this` mobilizer from a vector `v_array` of generalized velocities for
   // the entire MultibodyTree model.
   // @pre @p v_array is of size MultibodyTree::num_velocities().
-  Eigen::VectorBlock<const Eigen::Ref<const VectorX<T>>>
+  Eigen::Ref<const VectorX<T>>
   get_velocities_from_array(const Eigen::Ref<const VectorX<T>>& v_array) const {
     DRAKE_DEMAND(
         v_array.size() == this->get_parent_tree().num_velocities());
@@ -544,7 +544,7 @@ class Mobilizer : public MultibodyElement<Mobilizer, T, MobilizerIndex> {
   }
 
   // Mutable version of get_velocities_from_array().
-  Eigen::VectorBlock<Eigen::Ref<VectorX<T>>> get_mutable_velocities_from_array(
+  Eigen::Ref<VectorX<T>> get_mutable_velocities_from_array(
       EigenPtr<VectorX<T>> v_array) const {
     DRAKE_DEMAND(v_array != nullptr);
     DRAKE_DEMAND(
@@ -558,15 +558,13 @@ class Mobilizer : public MultibodyElement<Mobilizer, T, MobilizerIndex> {
   // generalized accelerations for the entire MultibodyTree model.
   // This method aborts if the input array is not of size
   // MultibodyTree::num_velocities().
-  Eigen::VectorBlock<const Eigen::Ref<const VectorX<T>>>
-  get_accelerations_from_array(
+  Eigen::Ref<const VectorX<T>> get_accelerations_from_array(
       const Eigen::Ref<const VectorX<T>>& vdot_array) const {
     return get_velocities_from_array(vdot_array);
   }
 
   // Mutable version of get_accelerations_from_array().
-  Eigen::VectorBlock<Eigen::Ref<VectorX<T>>>
-  get_mutable_accelerations_from_array(
+  Eigen::Ref<VectorX<T>> get_mutable_accelerations_from_array(
       EigenPtr<VectorX<T>> vdot_array) const {
     return get_mutable_velocities_from_array(vdot_array);
   }
@@ -576,15 +574,13 @@ class Mobilizer : public MultibodyElement<Mobilizer, T, MobilizerIndex> {
   // entire MultibodyTree model.
   // This method aborts if the input array is not of size
   // MultibodyTree::num_velocities().
-  Eigen::VectorBlock<const Eigen::Ref<const VectorX<T>>>
-  get_generalized_forces_from_array(
+  Eigen::Ref<const VectorX<T>> get_generalized_forces_from_array(
       const Eigen::Ref<const VectorX<T>>& tau_array) const {
     return get_velocities_from_array(tau_array);
   }
 
   // Mutable version of get_generalized_forces_from_array().
-  Eigen::VectorBlock<Eigen::Ref<VectorX<T>>>
-  get_mutable_generalized_forces_from_array(
+  Eigen::Ref<VectorX<T>> get_mutable_generalized_forces_from_array(
       EigenPtr<VectorX<T>> tau_array) const {
     return get_mutable_velocities_from_array(tau_array);
   }

multibody/tree/multibody_tree.cc

@@ -2882,9 +2882,7 @@ void MultibodyTree<T>::CalcArticulatedBodyForceCache(
       const BodyNode<T>& node = *body_nodes_[body_node_index];
 
       // Get generalized force and body force for this node.
-      // N.B. Using the VectorBlock here avoids heap allocation. We have
-      // observed this to penalize performance for large models (nv > 36).
-      Eigen::VectorBlock<const Eigen::Ref<const VectorX<T>>> tau_applied =
+      Eigen::Ref<const VectorX<T>> tau_applied =
           node.get_mobilizer().get_generalized_forces_from_array(
               generalized_forces);
       const SpatialForce<T>& Fapplied_Bo_W = body_forces[body_node_index];

multibody/tree/planar_joint.h

@@ -267,7 +267,7 @@ class PlanarJoint final : public Joint<T> {
                        const T& joint_tau,
                        MultibodyForces<T>* forces) const final {
     DRAKE_DEMAND(joint_dof < 3);
-    Eigen::VectorBlock<Eigen::Ref<VectorX<T>>> tau_mob =
+    Eigen::Ref<VectorX<T>> tau_mob =
         get_mobilizer()->get_mutable_generalized_forces_from_array(
             &forces->mutable_generalized_forces());
     tau_mob(joint_dof) += joint_tau;
@@ -279,7 +279,7 @@ class PlanarJoint final : public Joint<T> {
   /// viscous law `f = -d⋅v`, with d the damping coefficient (see damping()).
   void DoAddInDamping(const systems::Context<T>& context,
                       MultibodyForces<T>* forces) const final {
-    Eigen::VectorBlock<Eigen::Ref<VectorX<T>>> tau =
+    Eigen::Ref<VectorX<T>> tau =
         get_mobilizer()->get_mutable_generalized_forces_from_array(
             &forces->mutable_generalized_forces());
     const Vector2<T>& v_translation = get_translational_velocity(context);

multibody/tree/prismatic_joint.h

@@ -239,7 +239,7 @@ class PrismaticJoint final : public Joint<T> {
       MultibodyForces<T>* forces) const final {
     // Right now we assume all the forces in joint_tau go into a single
     // mobilizer.
-    Eigen::VectorBlock<Eigen::Ref<VectorX<T>>> tau_mob =
+    Eigen::Ref<VectorX<T>> tau_mob =
         get_mobilizer()->get_mutable_generalized_forces_from_array(
             &forces->mutable_generalized_forces());
     tau_mob(joint_dof) += joint_tau;

multibody/tree/revolute_joint.h

@@ -272,7 +272,7 @@ class RevoluteJoint final : public Joint<T> {
     // Right now we assume all the forces in joint_tau go into a single
     // mobilizer.
     DRAKE_DEMAND(joint_dof == 0);
-    Eigen::VectorBlock<Eigen::Ref<VectorX<T>>> tau_mob =
+    Eigen::Ref<VectorX<T>> tau_mob =
         get_mobilizer()->get_mutable_generalized_forces_from_array(
             &forces->mutable_generalized_forces());
     tau_mob(joint_dof) += joint_tau;

multibody/tree/screw_joint.h

@@ -251,7 +251,7 @@ class ScrewJoint final : public Joint<T> {
                        const T& joint_tau,
                        MultibodyForces<T>* forces) const final {
     DRAKE_DEMAND(joint_dof < 1);
-    Eigen::VectorBlock<Eigen::Ref<VectorX<T>>> tau_mob =
+    Eigen::Ref<VectorX<T>> tau_mob =
         get_mobilizer()->get_mutable_generalized_forces_from_array(
             &forces->mutable_generalized_forces());
     tau_mob(joint_dof) += joint_tau;
@@ -263,7 +263,7 @@ class ScrewJoint final : public Joint<T> {
     viscous law `f = -d⋅v`, with d the damping coefficient (see damping()). */
   void DoAddInDamping(const systems::Context<T>& context,
                       MultibodyForces<T>* forces) const final {
-    Eigen::VectorBlock<Eigen::Ref<VectorX<T>>> tau =
+    Eigen::Ref<VectorX<T>> tau =
         get_mobilizer()->get_mutable_generalized_forces_from_array(
             &forces->mutable_generalized_forces());
     const T& v_angular = get_angular_velocity(context);

multibody/tree/universal_joint.h

@@ -189,7 +189,7 @@ class UniversalJoint final : public Joint<T> {
                        const T& joint_tau,
                        MultibodyForces<T>* forces) const override {
     DRAKE_DEMAND(joint_dof < 2);
-    Eigen::VectorBlock<Eigen::Ref<VectorX<T>>> tau_mob =
+    Eigen::Ref<VectorX<T>> tau_mob =
         get_mobilizer()->get_mutable_generalized_forces_from_array(
             &forces->mutable_generalized_forces());
     tau_mob(joint_dof) += joint_tau;
@@ -201,7 +201,7 @@ class UniversalJoint final : public Joint<T> {
   /// viscous law `τ = -d⋅ω`, with d the damping coefficient (see damping()).
   void DoAddInDamping(const systems::Context<T>& context,
                       MultibodyForces<T>* forces) const override {
-    Eigen::VectorBlock<Eigen::Ref<VectorX<T>>> tau =
+    Eigen::Ref<VectorX<T>> tau =
         get_mobilizer()->get_mutable_generalized_forces_from_array(
             &forces->mutable_generalized_forces());
     const Vector2<T>& theta_dot = get_angular_rates(context);