From 0397f9a5045bb8db4af357d345d8306b0ff2312d Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Tue, 27 Jul 2021 08:28:16 -0700 Subject: [PATCH] [multibody] Avoid Eigen::Block> (#15451) * [multibody] Avoid Eigen::Block> 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 | 2 +- multibody/tree/body_node.h | 28 +++------------------------- multibody/tree/mobilizer.h | 22 +++++++++------------- multibody/tree/multibody_tree.cc | 4 +--- multibody/tree/planar_joint.h | 4 ++-- multibody/tree/prismatic_joint.h | 2 +- multibody/tree/revolute_joint.h | 2 +- multibody/tree/screw_joint.h | 4 ++-- multibody/tree/universal_joint.h | 4 ++-- 9 files changed, 22 insertions(+), 50 deletions(-) diff --git a/multibody/tree/ball_rpy_joint.h b/multibody/tree/ball_rpy_joint.h index b09b61c03a12..3741afb7c195 100644 --- a/multibody/tree/ball_rpy_joint.h +++ b/multibody/tree/ball_rpy_joint.h @@ -201,7 +201,7 @@ class BallRpyJoint final : public Joint { /// viscous law `τ = -d⋅ω`, with d the damping coefficient (see damping()). void DoAddInDamping(const systems::Context& context, MultibodyForces* forces) const override { - Eigen::VectorBlock>> t_BMo_F = + Eigen::Ref> t_BMo_F = get_mobilizer()->get_mutable_generalized_forces_from_array( &forces->mutable_generalized_forces()); const Vector3& w_FM = get_angular_velocity(context); diff --git a/multibody/tree/body_node.h b/multibody/tree/body_node.h index 28d3329b01c9..90c3b04ccfae 100644 --- a/multibody/tree/body_node.h +++ b/multibody/tree/body_node.h @@ -1599,20 +1599,12 @@ class BodyNode : public MultibodyElement { // 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> get_accelerations( - AccelerationKinematicsCache* ac) const { - const VectorX& vdot = ac->get_vdot(); - return get_velocities_from_array(vdot); - } - - // Mutable version of get_accelerations_from_array(). - Eigen::VectorBlock>> get_mutable_accelerations( + Eigen::Ref> get_mutable_accelerations( AccelerationKinematicsCache* ac) const { VectorX& vdot = ac->get_mutable_vdot(); return get_mutable_velocities_from_array(&vdot); } - // ========================================================================= // ArticulatedBodyInertiaCache Accessors and Mutators. @@ -1750,14 +1742,7 @@ class BodyNode : public MultibodyElement { // 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>> - get_velocities_from_array(const Eigen::Ref>& v) const { - return v.segment(topology_.mobilizer_velocities_start_in_v, - topology_.num_mobilizer_velocities); - } - - // Mutable version of get_velocities_from_array(). - Eigen::VectorBlock>> get_mutable_velocities_from_array( + Eigen::Ref> get_mutable_velocities_from_array( EigenPtr> v) const { DRAKE_ASSERT(v != nullptr); return v->segment(topology_.mobilizer_velocities_start_in_v, @@ -1767,14 +1752,7 @@ class BodyNode : public MultibodyElement { // 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> get_generalized_forces_from_array( - const VectorX& tau) const { - return get_velocities_from_array(tau); - } - - // Mutable version of get_generalized_forces_from_array() - Eigen::VectorBlock>> - get_mutable_generalized_forces_from_array( + Eigen::Ref> get_mutable_generalized_forces_from_array( EigenPtr> tau) const { DRAKE_ASSERT(tau != nullptr); return get_mutable_velocities_from_array(tau); diff --git a/multibody/tree/mobilizer.h b/multibody/tree/mobilizer.h index 6c7c73b186a2..227be2043ec7 100644 --- a/multibody/tree/mobilizer.h +++ b/multibody/tree/mobilizer.h @@ -513,8 +513,8 @@ class Mobilizer : public MultibodyElement { // 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>> - get_positions_from_array(const Eigen::Ref>& q_array) const { + Eigen::Ref> get_positions_from_array( + const Eigen::Ref>& q_array) const { DRAKE_DEMAND( q_array.size() == this->get_parent_tree().num_positions()); return q_array.segment(topology_.positions_start, @@ -522,7 +522,7 @@ class Mobilizer : public MultibodyElement { } // Mutable version of get_positions_from_array(). - Eigen::VectorBlock>> get_mutable_positions_from_array( + Eigen::Ref> get_mutable_positions_from_array( EigenPtr> q_array) const { DRAKE_DEMAND(q_array != nullptr); DRAKE_DEMAND( @@ -535,7 +535,7 @@ class Mobilizer : public MultibodyElement { // 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>> + Eigen::Ref> get_velocities_from_array(const Eigen::Ref>& v_array) const { DRAKE_DEMAND( v_array.size() == this->get_parent_tree().num_velocities()); @@ -544,7 +544,7 @@ class Mobilizer : public MultibodyElement { } // Mutable version of get_velocities_from_array(). - Eigen::VectorBlock>> get_mutable_velocities_from_array( + Eigen::Ref> get_mutable_velocities_from_array( EigenPtr> v_array) const { DRAKE_DEMAND(v_array != nullptr); DRAKE_DEMAND( @@ -558,15 +558,13 @@ class Mobilizer : public MultibodyElement { // generalized accelerations for the entire MultibodyTree model. // This method aborts if the input array is not of size // MultibodyTree::num_velocities(). - Eigen::VectorBlock>> - get_accelerations_from_array( + Eigen::Ref> get_accelerations_from_array( const Eigen::Ref>& vdot_array) const { return get_velocities_from_array(vdot_array); } // Mutable version of get_accelerations_from_array(). - Eigen::VectorBlock>> - get_mutable_accelerations_from_array( + Eigen::Ref> get_mutable_accelerations_from_array( EigenPtr> vdot_array) const { return get_mutable_velocities_from_array(vdot_array); } @@ -576,15 +574,13 @@ class Mobilizer : public MultibodyElement { // entire MultibodyTree model. // This method aborts if the input array is not of size // MultibodyTree::num_velocities(). - Eigen::VectorBlock>> - get_generalized_forces_from_array( + Eigen::Ref> get_generalized_forces_from_array( const Eigen::Ref>& tau_array) const { return get_velocities_from_array(tau_array); } // Mutable version of get_generalized_forces_from_array(). - Eigen::VectorBlock>> - get_mutable_generalized_forces_from_array( + Eigen::Ref> get_mutable_generalized_forces_from_array( EigenPtr> tau_array) const { return get_mutable_velocities_from_array(tau_array); } diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc index 24b9676f0e07..d76fbd692e06 100644 --- a/multibody/tree/multibody_tree.cc +++ b/multibody/tree/multibody_tree.cc @@ -2882,9 +2882,7 @@ void MultibodyTree::CalcArticulatedBodyForceCache( const BodyNode& 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>> tau_applied = + Eigen::Ref> tau_applied = node.get_mobilizer().get_generalized_forces_from_array( generalized_forces); const SpatialForce& Fapplied_Bo_W = body_forces[body_node_index]; diff --git a/multibody/tree/planar_joint.h b/multibody/tree/planar_joint.h index 30bcac5a3395..889ef559751b 100644 --- a/multibody/tree/planar_joint.h +++ b/multibody/tree/planar_joint.h @@ -267,7 +267,7 @@ class PlanarJoint final : public Joint { const T& joint_tau, MultibodyForces* forces) const final { DRAKE_DEMAND(joint_dof < 3); - Eigen::VectorBlock>> tau_mob = + Eigen::Ref> 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 { /// viscous law `f = -d⋅v`, with d the damping coefficient (see damping()). void DoAddInDamping(const systems::Context& context, MultibodyForces* forces) const final { - Eigen::VectorBlock>> tau = + Eigen::Ref> tau = get_mobilizer()->get_mutable_generalized_forces_from_array( &forces->mutable_generalized_forces()); const Vector2& v_translation = get_translational_velocity(context); diff --git a/multibody/tree/prismatic_joint.h b/multibody/tree/prismatic_joint.h index 56f555a1836e..4ba2f724b6fe 100644 --- a/multibody/tree/prismatic_joint.h +++ b/multibody/tree/prismatic_joint.h @@ -239,7 +239,7 @@ class PrismaticJoint final : public Joint { MultibodyForces* forces) const final { // Right now we assume all the forces in joint_tau go into a single // mobilizer. - Eigen::VectorBlock>> tau_mob = + Eigen::Ref> tau_mob = get_mobilizer()->get_mutable_generalized_forces_from_array( &forces->mutable_generalized_forces()); tau_mob(joint_dof) += joint_tau; diff --git a/multibody/tree/revolute_joint.h b/multibody/tree/revolute_joint.h index 68132e13848c..957a7a5088f4 100644 --- a/multibody/tree/revolute_joint.h +++ b/multibody/tree/revolute_joint.h @@ -272,7 +272,7 @@ class RevoluteJoint final : public Joint { // Right now we assume all the forces in joint_tau go into a single // mobilizer. DRAKE_DEMAND(joint_dof == 0); - Eigen::VectorBlock>> tau_mob = + Eigen::Ref> tau_mob = get_mobilizer()->get_mutable_generalized_forces_from_array( &forces->mutable_generalized_forces()); tau_mob(joint_dof) += joint_tau; diff --git a/multibody/tree/screw_joint.h b/multibody/tree/screw_joint.h index 0289e23c86bd..16aa39555753 100644 --- a/multibody/tree/screw_joint.h +++ b/multibody/tree/screw_joint.h @@ -251,7 +251,7 @@ class ScrewJoint final : public Joint { const T& joint_tau, MultibodyForces* forces) const final { DRAKE_DEMAND(joint_dof < 1); - Eigen::VectorBlock>> tau_mob = + Eigen::Ref> 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 { viscous law `f = -d⋅v`, with d the damping coefficient (see damping()). */ void DoAddInDamping(const systems::Context& context, MultibodyForces* forces) const final { - Eigen::VectorBlock>> tau = + Eigen::Ref> tau = get_mobilizer()->get_mutable_generalized_forces_from_array( &forces->mutable_generalized_forces()); const T& v_angular = get_angular_velocity(context); diff --git a/multibody/tree/universal_joint.h b/multibody/tree/universal_joint.h index 9ae06e6142f2..639b5e27b37e 100644 --- a/multibody/tree/universal_joint.h +++ b/multibody/tree/universal_joint.h @@ -189,7 +189,7 @@ class UniversalJoint final : public Joint { const T& joint_tau, MultibodyForces* forces) const override { DRAKE_DEMAND(joint_dof < 2); - Eigen::VectorBlock>> tau_mob = + Eigen::Ref> 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 { /// viscous law `τ = -d⋅ω`, with d the damping coefficient (see damping()). void DoAddInDamping(const systems::Context& context, MultibodyForces* forces) const override { - Eigen::VectorBlock>> tau = + Eigen::Ref> tau = get_mobilizer()->get_mutable_generalized_forces_from_array( &forces->mutable_generalized_forces()); const Vector2& theta_dot = get_angular_rates(context);