Some speedups for inertia * vector (RobotLocomotion#22287)

multibody/tree/rotational_inertia.h

@@ -420,8 +420,26 @@ class RotationalInertia {
   /// @param w_E Vector to post-multiply with `this` rotational inertia.
   /// @return The Vector that results from multiplying `this` by `w_E`.
   // TODO(Mitiguy) Issue #6145, add direct unit test for this method.
+  // TODO(sherm1) Consider promoting this to a general utility if there
+  //  are similar symmetric*vector cases elsewhere.
   Vector3<T> operator*(const Vector3<T>& w_E) const {
-    return Vector3<T>(get_symmetric_matrix_view() * w_E);
+    // Eigen's symmetric multiply can be slow. Do this by hand instead:
+    //     [a (b) (c)]   [x]   [ ax+by+cz ]
+    //     [b  d  (e)] * [y] = [ bx+dy+ez ]
+    //     [c  e   f ]   [z]   [ cx+ey+fz ]
+    const T& a = I_SP_E_(0, 0);  // Access only lower triangle.
+    const T& b = I_SP_E_(1, 0);
+    const T& c = I_SP_E_(2, 0);
+    const T& d = I_SP_E_(1, 1);
+    const T& e = I_SP_E_(2, 1);
+    const T& f = I_SP_E_(2, 2);
+    const T& x = w_E(0);
+    const T& y = w_E(1);
+    const T& z = w_E(2);
+
+    const Vector3<T> Iw(a * x + b * y + c * z, b * x + d * y + e * z,
+                        c * x + e * y + f * z);
+    return Iw;
   }
 
   /// Divides `this` rotational inertia by a positive scalar (> 0).

multibody/tree/spatial_inertia.cc

@@ -441,35 +441,35 @@ SpatialForce<T> SpatialInertia<T>::operator*(
     const SpatialAcceleration<T>& A_WB_E) const {
   const Vector3<T>& alpha_WB_E = A_WB_E.rotational();
   const Vector3<T>& a_WBo_E = A_WB_E.translational();
-  const Vector3<T>& mp_BoBcm_E = CalcComMoment();  // = m * p_BoBcm
   // Return (see class's documentation):
   // ⌈ tau_Bo_E ⌉   ⌈    I_Bo_E     | m * p_BoBcm× ⌉   ⌈ alpha_WB_E ⌉
   // |          | = |               |              | * |            |
   // ⌊  f_Bo_E  ⌋   ⌊ -m * p_BoBcm× |   m * Id     ⌋   ⌊  a_WBo_E   ⌋
   return SpatialForce<T>(
-      /* rotational */
-      CalcRotationalInertia() * alpha_WB_E + mp_BoBcm_E.cross(a_WBo_E),
-      /* translational: notice the order of the cross product is the reversed
-       * of the documentation above and thus no minus sign is needed. */
-      alpha_WB_E.cross(mp_BoBcm_E) + get_mass() * a_WBo_E);
+      // Note: p_PScm_E here is p_BoBcm in the above notation.
+      // Rotational
+      mass_ * (G_SP_E_ * alpha_WB_E + p_PScm_E_.cross(a_WBo_E)),
+      // Translational: notice the order of the cross product is the reversed
+      // of the documentation above and thus no minus sign is needed.
+      mass_ * (alpha_WB_E.cross(p_PScm_E_) + a_WBo_E));
 }
 
 template <typename T>
 SpatialMomentum<T> SpatialInertia<T>::operator*(
     const SpatialVelocity<T>& V_WBp_E) const {
   const Vector3<T>& w_WB_E = V_WBp_E.rotational();
   const Vector3<T>& v_WP_E = V_WBp_E.translational();
-  const Vector3<T>& mp_BoBcm_E = CalcComMoment();  // = m * p_BoBcm
   // Return (see class's documentation):
   // ⌈ h_WB  ⌉   ⌈     I_Bp      | m * p_BoBcm× ⌉   ⌈ w_WB ⌉
   // |       | = |               |              | * |      |
   // ⌊ l_WBp ⌋   ⌊ -m * p_BoBcm× |   m * Id     ⌋   ⌊ v_WP ⌋
   return SpatialMomentum<T>(
+      // Note: p_PScm_E here is p_BoBcm in the above notation.
       // Rotational
-      CalcRotationalInertia() * w_WB_E + mp_BoBcm_E.cross(v_WP_E),
+      mass_ * (G_SP_E_ * w_WB_E + p_PScm_E_.cross(v_WP_E)),
       // Translational: notice the order of the cross product is the reversed
       // of the documentation above and thus no minus sign is needed.
-      w_WB_E.cross(mp_BoBcm_E) + get_mass() * v_WP_E);
+      mass_ * (w_WB_E.cross(p_PScm_E_) + v_WP_E));
 }
 
 template <typename T>

multibody/tree/test/spatial_inertia_test.cc

@@ -1194,7 +1194,8 @@ GTEST_TEST(SpatialInertia, KineticEnergy) {
   const double ke_WB_expected =
       0.5 * w_WB.dot(I_Bcm_W * w_WB) + 0.5 * mass * v_WBcm.squaredNorm();
 
-  EXPECT_NEAR(ke_WB, ke_WB_expected, 50 * kEpsilon);
+  const double relative_tol = ke_WB_expected * (8 * kEpsilon);
+  EXPECT_NEAR(ke_WB, ke_WB_expected, relative_tol);
 }
 
 GTEST_TEST(SpatialInertia, MultiplyByEigenMatrix) {