diff --git a/multibody/tree/rotational_inertia.h b/multibody/tree/rotational_inertia.h index ee1a9e1a7621..ce665d0fada1 100644 --- a/multibody/tree/rotational_inertia.h +++ b/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 operator*(const Vector3& w_E) const { - return Vector3(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 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). diff --git a/multibody/tree/spatial_inertia.cc b/multibody/tree/spatial_inertia.cc index 724af3f63221..8a026298411f 100644 --- a/multibody/tree/spatial_inertia.cc +++ b/multibody/tree/spatial_inertia.cc @@ -441,17 +441,17 @@ SpatialForce SpatialInertia::operator*( const SpatialAcceleration& A_WB_E) const { const Vector3& alpha_WB_E = A_WB_E.rotational(); const Vector3& a_WBo_E = A_WB_E.translational(); - const Vector3& 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( - /* 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 @@ -459,17 +459,17 @@ SpatialMomentum SpatialInertia::operator*( const SpatialVelocity& V_WBp_E) const { const Vector3& w_WB_E = V_WBp_E.rotational(); const Vector3& v_WP_E = V_WBp_E.translational(); - const Vector3& 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( + // 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 diff --git a/multibody/tree/test/spatial_inertia_test.cc b/multibody/tree/test/spatial_inertia_test.cc index 3286bcd1b792..64ebc9bafb1a 100644 --- a/multibody/tree/test/spatial_inertia_test.cc +++ b/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) {