New SpatialInertia factory method SolidBoxWithMass(). (#19141)

Co-Authored-By: mitiguy <[email protected]>

bindings/pydrake/multibody/test/mesh_to_model_test.py

@@ -224,7 +224,7 @@ def test_mass(self):
         rho = 1.5
         density_mass = V * rho
         expected_mass = density_mass + 1
-        G_GGo_G = UnitInertia.SolidBox(2, 2, 2)
+        G_GGo_G = UnitInertia.SolidCube(2)
         I_GGo_G_density = G_GGo_G * density_mass
         I_GGo_G_mass = G_GGo_G * expected_mass
 

multibody/benchmarks/inclined_plane/inclined_plane_plant.cc

@@ -86,10 +86,8 @@ void AddInclinedPlaneWithBlockToPlant(
   DRAKE_THROW_UNLESS(LBx > 0 && LBy > 0 && LBz > 0 && massB > 0);
 
   // Describe body B's mass, center of mass, and inertia properties.
-  const Vector3<double> p_BoBcm_B = Vector3<double>::Zero();
-  const UnitInertia<double> G_BBcm_B =
-      UnitInertia<double>::SolidBox(LBx, LBy, LBz);
-  const SpatialInertia<double> M_BBcm_B(massB, p_BoBcm_B, G_BBcm_B);
+  const SpatialInertia<double> M_BBcm_B =
+      SpatialInertia<double>::SolidBoxWithMass(massB, LBx, LBy, LBz);
 
   // Create a rigid body B with the mass properties of a uniform solid block.
   const RigidBody<double>& blockB = plant->AddRigidBody("BodyB", M_BBcm_B);

multibody/plant/test/joint_limits_test.cc

@@ -72,7 +72,7 @@ GTEST_TEST(JointLimitsTest, PrismaticJointConvergenceTest) {
         Vector3<double>::Zero());
     const auto M_B = SpatialInertia<double>::MakeFromCentralInertia(
         mass, Vector3<double>::Zero(),
-        mass * UnitInertia<double>::SolidBox(box_size, box_size, box_size));
+        mass * UnitInertia<double>::SolidCube(box_size));
     const RigidBody<double>& body = plant.AddRigidBody("Body", M_B);
     const PrismaticJoint<double>& slider = plant.AddJoint<PrismaticJoint>(
         "Slider", plant.world_body(), std::nullopt, body, std::nullopt,

multibody/plant/test/multibody_plant_forward_dynamics_test.cc

@@ -217,8 +217,7 @@ GTEST_TEST(WeldedBoxesTest, ForwardDynamicsViaArticulatedBodyAlgorithm) {
 
   // Set a model with two boxes anchored to the world via weld joints.
   const Vector3d p_BoBcm_B = Vector3d::Zero();
-  const UnitInertia<double> G_BBcm =
-      UnitInertia<double>::SolidBox(kCubeSize, kCubeSize, kCubeSize);
+  const UnitInertia<double> G_BBcm = UnitInertia<double>::SolidCube(kCubeSize);
   const SpatialInertia<double> M_BBo_B =
       SpatialInertia<double>::MakeFromCentralInertia(kBoxMass, p_BoBcm_B,
                                                      G_BBcm);

multibody/plant/test/multibody_plant_jacobians_test.cc

@@ -363,10 +363,9 @@ class TwoDOFPlanarPendulumTest : public ::testing::Test {
   void SetUp() override {
     // Set a spatial inertia for each link.  For now, these are unimportant
     // because this fixture is only used for kinematic tests (e.g., Jacobians).
-    const UnitInertia<double> G_Bcm =
-        UnitInertia<double>::SolidBox(link_length_, 1, 1);
-    const Vector3<double> p_BoBcm_B = Vector3<double>::Zero();
-    const SpatialInertia<double> M_Bcm(mass_link_, p_BoBcm_B, G_Bcm);
+    const SpatialInertia<double> M_Bcm =
+        SpatialInertia<double>::SolidBoxWithMass(
+            mass_link_, link_length_, 1, 1);
 
     // Create an empty MultibodyPlant and then add the two links.
     plant_ = std::make_unique<MultibodyPlant<double>>(0.0);

multibody/plant/test/multibody_plant_reaction_forces_test.cc

@@ -628,7 +628,7 @@ class WeldedBoxesTest : public ::testing::Test {
   void AddBoxes() {
     const Vector3d p_BoBcm_B = Vector3d::Zero();
     const UnitInertia<double> G_BBcm =
-        UnitInertia<double>::SolidBox(kCubeSize, kCubeSize, kCubeSize);
+        UnitInertia<double>::SolidCube(kCubeSize);
     const SpatialInertia<double> M_BBo_B =
         SpatialInertia<double>::MakeFromCentralInertia(kBoxMass, p_BoBcm_B,
                                                        G_BBcm);

multibody/plant/test/multibody_plant_test.cc

@@ -3463,7 +3463,7 @@ GTEST_TEST(MultibodyPlantTest, RigidBodyParameters) {
   const double cube_mass = 5.0;
   const Vector3d cube_com(0, 0, 0);
   const UnitInertia<double> cube_unit_inertia =
-      UnitInertia<double>::SolidBox(cube_length, cube_length, cube_length);
+      UnitInertia<double>::SolidCube(cube_length);
   const RigidBody<double>& cube = plant.AddRigidBody(
       "cube", SpatialInertia<double>(cube_mass, cube_com, cube_unit_inertia));
 
@@ -3518,8 +3518,7 @@ GTEST_TEST(MultibodyPlantTest, RigidBodyParameters) {
   const double new_cube_mass = 3.0;
   const Vector3d new_cube_com(0, 0, 0);
   const UnitInertia<double> new_cube_unit_inertia =
-      UnitInertia<double>::SolidBox(new_cube_length, new_cube_length,
-                                    new_cube_length);
+      UnitInertia<double>::SolidCube(new_cube_length);
 
   SpatialInertia<double> new_sphere_params(new_sphere_mass, new_sphere_com,
                                            new_sphere_unit_inertia);

multibody/tree/spatial_inertia.cc

@@ -20,7 +20,7 @@ SpatialInertia<T> SpatialInertia<T>::PointMass(
     const T& mass, const Vector3<T>& position) {
   // Ensure mass is non-negative.
   if (mass < 0) {
-    std::string error_message = fmt::format(
+    const std::string error_message = fmt::format(
         "{}(): The mass of a particle is negative: {}.", __func__, mass);
     throw std::logic_error(error_message);
   }
@@ -41,64 +41,81 @@ SpatialInertia<T> SpatialInertia<T>::SolidBoxWithDensity(
     const T& density, const T& lx, const T& ly, const T& lz) {
   // Ensure lx, ly, lz are positive.
   if (lx <= 0 || ly <= 0 || lz <= 0) {
-    std::string error_message = fmt::format("{}(): One or more dimensions of a "
-    "solid box is negative or zero: ({}, {}, {}).",
-      __func__, lx, ly, lz);
+    const std::string error_message = fmt::format(
+        "{}(): One or more dimensions of a solid box is negative or zero: "
+        "({}, {}, {}).", __func__, lx, ly, lz);
     throw std::logic_error(error_message);
   }
 
   const T volume = lx * ly * lz;
   const T mass = density * volume;
+  return SpatialInertia<T>::SolidBoxWithMass(mass, lx, ly, lz);
+}
+
+template <typename T>
+SpatialInertia<T> SpatialInertia<T>::SolidBoxWithMass(
+    const T& mass, const T& lx, const T& ly, const T& lz) {
+  // Ensure lx, ly, lz are positive.
+  if (lx <= 0 || ly <= 0 || lz <= 0) {
+    const std::string error_message = fmt::format(
+        "{}(): One or more dimensions of a solid box is negative or zero: "
+        "({}, {}, {}).", __func__, lx, ly, lz);
+    throw std::logic_error(error_message);
+  }
   const Vector3<T> p_BoBcm_B = Vector3<T>::Zero();
   const UnitInertia<T> G_BBo_B = UnitInertia<T>::SolidBox(lx, ly, lz);
   return SpatialInertia<T>(mass, p_BoBcm_B, G_BBo_B);
 }
 
 template <typename T>
 SpatialInertia<T> SpatialInertia<T>::SolidCubeWithDensity(
-    const T& density, const T& l) {
-  // Ensure l is positive.
-  if (l <= 0) {
-    std::string error_message = fmt::format(
+    const T& density, const T& length) {
+  // Ensure length is positive.
+  if (length <= 0) {
+    const std::string error_message = fmt::format(
         "{}(): The length of a solid cube is negative or zero: {}.",
-        __func__, l);
+        __func__, length);
     throw std::logic_error(error_message);
   }
 
-  const T volume = l * l * l;
+  const T volume = length * length * length;
   const T mass = density * volume;
   const Vector3<T> p_BoBcm_B = Vector3<T>::Zero();
-  const UnitInertia<T> G_BBo_B = UnitInertia<T>::SolidCube(l);
+  const UnitInertia<T> G_BBo_B = UnitInertia<T>::SolidCube(length);
   return SpatialInertia<T>(mass, p_BoBcm_B, G_BBo_B);
 }
 
 template <typename T>
 SpatialInertia<T> SpatialInertia<T>::SolidCapsuleWithDensity(
-    const T& density, const T& r, const T& l, const Vector3<T>& unit_vector) {
-  // Ensure r and l are positive.
-  if (r <= 0 || l <= 0) {
-    std::string error_message = fmt::format("{}(): A solid capsule's "
-      "radius = {} or length = {} is negative or zero.", __func__, r, l);
+    const T& density, const T& radius, const T& length,
+    const Vector3<T>& unit_vector) {
+  // Ensure radius and length are positive.
+  if (radius <= 0 || length <= 0) {
+    const std::string error_message = fmt::format(
+        "{}(): A solid capsule's radius = {} or length = {} is "
+        "negative or zero.", __func__, radius, length);
     throw std::logic_error(error_message);
   }
 
   // Volume = π r² L + 4/3 π r³
-  const T pi_r_squared = M_PI * r * r;
-  const T volume = pi_r_squared * l + (4.0 / 3.0) * pi_r_squared * r;
+  const T pi_r_squared = M_PI * radius * radius;
+  const T volume = pi_r_squared * length + (4.0 / 3.0) * pi_r_squared * radius;
   const T mass = density * volume;
   const Vector3<T> p_BoBcm_B = Vector3<T>::Zero();
   const UnitInertia<T> G_BBo_B =
-      UnitInertia<T>::SolidCapsule(r, l, unit_vector);
+      UnitInertia<T>::SolidCapsule(radius, length, unit_vector);
   return SpatialInertia<T>(mass, p_BoBcm_B, G_BBo_B);
 }
 
 template <typename T>
 SpatialInertia<T> SpatialInertia<T>::SolidCylinderWithDensity(
-    const T& density, const T& r, const T& l, const Vector3<T>& unit_vector) {
-  // Ensure r and l are positive.
-  if (r <= 0 || l <= 0) {
-    std::string error_message = fmt::format("{}(): A solid cylinder's "
-      "radius = {} or length = {} is negative or zero.", __func__, r, l);
+    const T& density, const T& radius, const T& length,
+    const Vector3<T>& unit_vector) {
+  // Ensure radius and length are positive.
+  if (radius <= 0 || length <= 0) {
+    const std::string error_message = fmt::format(
+        "{}(): A solid cylinder's radius = {} or length = {} is "
+        "negative or zero.", __func__, radius, length);
     throw std::logic_error(error_message);
   }
 
@@ -112,7 +129,7 @@ SpatialInertia<T> SpatialInertia<T>::SolidCylinderWithDensity(
                     __func__, fmt_eigen(unit_vector.transpose())));
   }
 
-  const T volume = M_PI * r * r * l;  // π r² l
+  const T volume = M_PI * radius * radius * length;  // π r² l
   const T mass = density * volume;
   const Vector3<T> p_BoBcm_B = Vector3<T>::Zero();
 
@@ -121,7 +138,7 @@ SpatialInertia<T> SpatialInertia<T>::SolidCylinderWithDensity(
   // UnitInertia::AxiallySymmetric() which normalizes unit_vector before use.
   // TODO(Mitiguy) remove normalization in UnitInertia::AxiallySymmetric().
   const UnitInertia<T> G_BBo_B =
-      UnitInertia<T>::SolidCylinder(r, l, unit_vector);
+      UnitInertia<T>::SolidCylinder(radius, length, unit_vector);
   return SpatialInertia<T>(mass, p_BoBcm_B, G_BBo_B);
 }
 
@@ -142,7 +159,7 @@ SpatialInertia<T> SpatialInertia<T>::ThinRodWithMass(
     const T& mass, const T& length, const Vector3<T>& unit_vector) {
   // Ensure mass and length are positive.
   if (mass <= 0 || length <= 0) {
-    std::string error_message = fmt::format(
+    const std::string error_message = fmt::format(
         "{}(): A thin rod's mass = {} or length = {} is negative or zero.",
         __func__, mass, length);
     throw std::logic_error(error_message);
@@ -175,7 +192,7 @@ SpatialInertia<T> SpatialInertia<T>::ThinRodWithMassAboutEnd(
     const T& mass, const T& length, const Vector3<T>& unit_vector) {
   // Ensure mass and length are positive.
   if (mass <= 0 || length <= 0) {
-    std::string error_message = fmt::format(
+    const std::string error_message = fmt::format(
         "{}(): A thin rod's mass = {} or length = {} is negative or zero.",
         __func__, mass, length);
     throw std::logic_error(error_message);
@@ -193,9 +210,9 @@ SpatialInertia<T> SpatialInertia<T>::SolidEllipsoidWithDensity(
     const T& density, const T& a, const T& b, const T& c) {
   // Ensure a, b, c are positive.
   if (a <= 0 || b <= 0 || c <= 0) {
-    std::string error_message = fmt::format("{}(): A solid ellipsoid's "
-      "semi-axis a = {} or b = {} or c = {} is negative or zero.",
-      __func__, a, b, c);
+    const std::string error_message = fmt::format(
+        "{}(): A solid ellipsoid's semi-axis a = {} or b = {} or c = {} "
+        "is negative or zero.", __func__, a, b, c);
     throw std::logic_error(error_message);
   }
   const T volume = (4.0 / 3.0) * M_PI * a * b * c;  // 4/3 π a b c
@@ -207,33 +224,35 @@ SpatialInertia<T> SpatialInertia<T>::SolidEllipsoidWithDensity(
 
 template <typename T>
 SpatialInertia<T> SpatialInertia<T>::SolidSphereWithDensity(
-    const T& density, const T& r) {
-  // Ensure r is positive.
-  if (r <= 0) {
-    std::string error_message = fmt::format("{}(): A solid sphere's "
-      "radius = {} is negative or zero.", __func__, r);
+    const T& density, const T& radius) {
+  // Ensure radius is positive.
+  if (radius <= 0) {
+    const std::string error_message = fmt::format(
+        "{}(): A solid sphere's radius = {} is negative or zero.",
+        __func__, radius);
     throw std::logic_error(error_message);
   }
-  const T volume = (4.0 / 3.0) * M_PI * r * r * r;  // 4/3 π r³
+  const T volume = (4.0 / 3.0) * M_PI * radius * radius * radius;  // 4/3 π r³
   const T mass = density * volume;
   const Vector3<T> p_BoBcm_B = Vector3<T>::Zero();
-  const UnitInertia<T> G_BBo_B = UnitInertia<T>::SolidSphere(r);
+  const UnitInertia<T> G_BBo_B = UnitInertia<T>::SolidSphere(radius);
   return SpatialInertia<T>(mass, p_BoBcm_B, G_BBo_B);
 }
 
 template <typename T>
 SpatialInertia<T> SpatialInertia<T>::HollowSphereWithDensity(
-    const T& area_density, const T& r) {
-  // Ensure r is positive.
-  if (r <= 0) {
-    std::string error_message = fmt::format("{}(): A hollow sphere's "
-      "radius = {} is negative or zero.", __func__, r);
+    const T& area_density, const T& radius) {
+  // Ensure radius is positive.
+  if (radius <= 0) {
+    const std::string error_message = fmt::format(
+        "{}(): A hollow sphere's radius = {} is negative or zero.",
+        __func__, radius);
     throw std::logic_error(error_message);
   }
-  const T area = 4.0 * M_PI * r * r;  // 4 π r²
+  const T area = 4.0 * M_PI * radius * radius;  // 4 π r²
   const T mass = area_density * area;
   const Vector3<T> p_BoBcm_B = Vector3<T>::Zero();
-  const UnitInertia<T> G_BBo_B = UnitInertia<T>::HollowSphere(r);
+  const UnitInertia<T> G_BBo_B = UnitInertia<T>::HollowSphere(radius);
   return SpatialInertia<T>(mass, p_BoBcm_B, G_BBo_B);
 }
 

multibody/tree/spatial_inertia.h

@@ -143,6 +143,17 @@ class SpatialInertia {
   static SpatialInertia<T> SolidBoxWithDensity(
       const T& density, const T& lx, const T& ly, const T& lz);
 
+  /// Creates a spatial inertia for a uniform density solid box B about its
+  /// geometric center Bo (which is coincident with B's center of mass Bcm).
+  /// @param[in] mass mass of the solid box (kg).
+  /// @param[in] lx length of the box in the Bx direction (meters).
+  /// @param[in] ly length of the box in the By direction (meters).
+  /// @param[in] lz length of the box in the Bz direction (meters).
+  /// @retval M_BBo_B B's spatial inertia about Bo, expressed in B.
+  /// @throws std::exception if any of lx, ly, lz are zero or negative.
+  static SpatialInertia<T> SolidBoxWithMass(
+      const T& mass, const T& lx, const T& ly, const T& lz);
+
   /// Creates a spatial inertia for a uniform density solid cube B about its
   /// geometric center Bo (which is coincident with B's center of mass Bcm).
   /// @param[in] density mass per volume (kg/m³).

multibody/tree/test/articulated_body_algorithm_test.cc

@@ -191,9 +191,9 @@ class FeatherstoneMobilizer final : public MobilizerImpl<T, 2, 2> {
 GTEST_TEST(ArticulatedBodyInertiaAlgorithm, FeatherstoneExample) {
   // Create box (B).
   const double Lx = 0.4, Ly = 1.0, Lz = 1.0;
-  const UnitInertia<double> G_Bcm = UnitInertia<double>::SolidBox(Lx, Ly, Lz);
   const double mass_box = 2.0;
-  const SpatialInertia<double> M_Bcm(mass_box, Vector3d::Zero(), G_Bcm);
+  const SpatialInertia<double> M_Bcm =
+      SpatialInertia<double>::SolidBoxWithMass(mass_box, Lx, Ly, Lz);
 
   // Create cylinder (C) in box.
   // Note that the unit inertia of the cylinder is taken about the x-axis.
@@ -268,9 +268,9 @@ GTEST_TEST(ArticulatedBodyInertiaAlgorithm, FeatherstoneExample) {
 GTEST_TEST(ArticulatedBodyInertiaAlgorithm, ModifiedFeatherstoneExample) {
   // Create box (B).
   const double Lx = 0.5, Ly = 1.2, Lz = 1.6;
-  const UnitInertia<double> G_Bcm = UnitInertia<double>::SolidBox(Lx, Ly, Lz);
   const double mass_box = 2.4;
-  const SpatialInertia<double> M_Bcm(mass_box, Vector3d::Zero(), G_Bcm);
+  const SpatialInertia<double> M_Bcm =
+      SpatialInertia<double>::SolidBoxWithMass(mass_box, Lx, Ly, Lz);
 
   // Create cylinder (C) in box.
   // Note that the unit inertia of the cylinder is taken about the x-axis.

multibody/tree/test/spatial_inertia_test.cc

@@ -98,7 +98,7 @@ GTEST_TEST(SpatialInertia, PointMass) {
 }
 
 // Tests the static method for the spatial inertia of a solid box.
-GTEST_TEST(SpatialInertia, SolidBoxWithDensity) {
+GTEST_TEST(SpatialInertia, SolidBoxWithDensityOrMass) {
   const double density = 1000;  // Water is 1 g/ml = 1000 kg/m³.
   const double lx = 1.0;
   const double ly = 2.0;
@@ -108,10 +108,16 @@ GTEST_TEST(SpatialInertia, SolidBoxWithDensity) {
   const Vector3<double> p_BoBcm_B = Vector3<double>::Zero();
   const UnitInertia<double>G_BBo_B = UnitInertia<double>::SolidBox(lx, ly, lz);
   const SpatialInertia<double> M_expected(mass, p_BoBcm_B, G_BBo_B);
-  const SpatialInertia<double> M =
+  const SpatialInertia<double> M_with_density =
       SpatialInertia<double>::SolidBoxWithDensity(density, lx, ly, lz);
-  EXPECT_TRUE(
-      CompareMatrices(M_expected.CopyToFullMatrix6(), M.CopyToFullMatrix6()));
+  EXPECT_TRUE(CompareMatrices(M_expected.CopyToFullMatrix6(),
+                              M_with_density.CopyToFullMatrix6()));
+
+  // Ensure SolidBoxWithDensity() matches SolidBoxWithMass().
+  const SpatialInertia<double> M_with_mass =
+      SpatialInertia<double>::SolidBoxWithMass(mass, lx, ly, lz);
+  EXPECT_TRUE(CompareMatrices(M_with_mass.CopyToFullMatrix6(),
+                              M_with_density.CopyToFullMatrix6()));
 
   // Ensure a negative or zero length, width, or height throws an exception.
   // There is not an exhaustive test of each parameter being zero or negative.
@@ -121,14 +127,26 @@ GTEST_TEST(SpatialInertia, SolidBoxWithDensity) {
       SpatialInertia<double>::SolidBoxWithDensity(density, 0, ly, lz),
       "[^]* One or more dimensions of a solid box is negative or zero: "
       "(.*, .*, .*).");
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      SpatialInertia<double>::SolidBoxWithMass(mass, 0, ly, lz),
+      "[^]* One or more dimensions of a solid box is negative or zero: "
+      "(.*, .*, .*).");
   DRAKE_EXPECT_THROWS_MESSAGE(
       SpatialInertia<double>::SolidBoxWithDensity(density, ly, -0.1, lz),
       "[^]* One or more dimensions of a solid box is negative or zero: "
       "(.*, .*, .*).");
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      SpatialInertia<double>::SolidBoxWithMass(mass, ly, -0.1, lz),
+      "[^]* One or more dimensions of a solid box is negative or zero: "
+      "(.*, .*, .*).");
   DRAKE_EXPECT_THROWS_MESSAGE(
       SpatialInertia<double>::SolidBoxWithDensity(density, ly, ly, -1E-15),
       "[^]* One or more dimensions of a solid box is negative or zero: "
       "(.*, .*, .*).");
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      SpatialInertia<double>::SolidBoxWithMass(mass, ly, ly, -1E-15),
+      "[^]* One or more dimensions of a solid box is negative or zero: "
+      "(.*, .*, .*).");
 }
 
 // Tests the static method for the spatial inertia of a solid cube.