Remove SpatialInertia::MakeTestCube() in favor of SpatialInertia::Mak…

…eUnitary() (#17547)

multibody/plant/test/compliant_contact_manager_test.cc

@@ -1668,8 +1668,8 @@ class MultiDofJointWithLimits final : public Joint<T> {
 GTEST_TEST(CompliantContactManager, ThrowForUnsupportedJoints) {
   MultibodyPlant<double> plant(1.0e-3);
   // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
-  const RigidBody<double>& body =
-      plant.AddRigidBody("DummyBody", SpatialInertia<double>::MakeTestCube());
+  const RigidBody<double>& body = plant.AddRigidBody("DummyBody",
+      SpatialInertia<double>::MakeUnitary());
   plant.AddJoint(std::make_unique<MultiDofJointWithLimits<double>>(
       plant.world_frame(), body.body_frame(), -1.0, 2.0));
   plant.Finalize();
@@ -1697,8 +1697,8 @@ GTEST_TEST(CompliantContactManager,
            VerifyMultiDofJointsWithoutLimitsAreSupported) {
   MultibodyPlant<double> plant(1.0e-3);
   // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
-  const RigidBody<double>& body =
-      plant.AddRigidBody("DummyBody", SpatialInertia<double>::MakeTestCube());
+  const RigidBody<double>& body = plant.AddRigidBody("DummyBody",
+      SpatialInertia<double>::MakeUnitary());
   const double kInf = std::numeric_limits<double>::infinity();
   plant.AddJoint(std::make_unique<MultiDofJointWithLimits<double>>(
       plant.world_frame(), body.body_frame(), -kInf, kInf));

multibody/plant/test/contact_results_to_lcm_test.cc

@@ -279,7 +279,7 @@ class ContactResultsToLcmTest : public ::testing::Test {
                      FullBodyName ref_name) {
     // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
     const auto& body = plant->AddRigidBody(body_name, model_index,
-        SpatialInertia<double>::MakeTestCube());
+        SpatialInertia<double>::MakeUnitary());
     /* The expected format based on knowledge of the ContactResultToLcmSystem's
      implementation. */
     body_names->push_back(fmt::format("{}({})", body_name, model_index));
@@ -852,7 +852,7 @@ class ConnectVisualizerTest : public ::testing::Test {
 
     // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
     const auto& body = plant_->AddRigidBody("link",
-        SpatialInertia<double>::MakeTestCube());
+        SpatialInertia<double>::MakeUnitary());
     plant_->RegisterCollisionGeometry(body, {}, Sphere(1.0), kGeoName,
                                       CoulombFriction<double>{});
     plant_->Finalize();

multibody/plant/test/discrete_update_manager_test.cc

@@ -163,7 +163,8 @@ class DiscreteUpdateManagerTest : public ::testing::Test {
  protected:
   void SetUp() override {
     // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
-    plant_.AddRigidBody("rigid body", SpatialInertia<double>::MakeTestCube());
+    plant_.AddRigidBody("rigid body",
+        SpatialInertia<double>::MakeUnitary());
     auto dummy_model = std::make_unique<DummyModel<double>>();
     dummy_model_ = dummy_model.get();
     plant_.AddPhysicalModel(std::move(dummy_model));

multibody/plant/test/joint_locking_test.cc

@@ -60,8 +60,8 @@ class JointLockingTest : public ::testing::TestWithParam<int> {
  private:
   void AddFloatingPendulum() {
     // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
-    plant_->AddRigidBody("body1", SpatialInertia<double>::MakeTestCube());
-    plant_->AddRigidBody("body2", SpatialInertia<double>::MakeTestCube());
+    plant_->AddRigidBody("body1", SpatialInertia<double>::MakeUnitary());
+    plant_->AddRigidBody("body2", SpatialInertia<double>::MakeUnitary());
 
     std::unique_ptr<RevoluteJoint<double>> body1_body2 =
         std::make_unique<RevoluteJoint<double>>(
@@ -73,8 +73,8 @@ class JointLockingTest : public ::testing::TestWithParam<int> {
 
   void AddDoublePendulum() {
     // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
-    plant_->AddRigidBody("body3", SpatialInertia<double>::MakeTestCube());
-    plant_->AddRigidBody("body4", SpatialInertia<double>::MakeTestCube());
+    plant_->AddRigidBody("body3", SpatialInertia<double>::MakeUnitary());
+    plant_->AddRigidBody("body4", SpatialInertia<double>::MakeUnitary());
 
     std::unique_ptr<RevoluteJoint<double>> world_body3 =
         std::make_unique<RevoluteJoint<double>>(

multibody/plant/test/multibody_plant_introspection_test.cc

@@ -150,7 +150,7 @@ GTEST_TEST(MultibodyPlantIntrospection, NonUniqueBaseBody) {
   // free.
   // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
   plant.AddRigidBody("free_body", default_model_instance(),
-      SpatialInertia<double>::MakeTestCube());
+      SpatialInertia<double>::MakeUnitary());
   const Body<double>& fixed_body = plant.AddRigidBody(
       "fixed_body", default_model_instance(), SpatialInertia<double>());
   plant.WeldFrames(plant.world_frame(), fixed_body.body_frame());

multibody/plant/test/multibody_plant_scalar_conversion_test.cc

@@ -40,7 +40,7 @@ TYPED_TEST_P(MultibodyPlantDefaultScalarsTest, RevoluteJointAndSpring) {
   MultibodyPlant<double> plant(0.0);
   // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
   const RigidBody<double>& body = plant.AddRigidBody("Body",
-      SpatialInertia<double>::MakeTestCube());
+      SpatialInertia<double>::MakeUnitary());
   const RevoluteJoint<double>& pin = plant.AddJoint<RevoluteJoint>(
       "Pin", plant.world_body(), std::nullopt, body, std::nullopt,
       Vector3<double>::UnitZ());

multibody/plant/test/multibody_plant_test.cc

@@ -1038,7 +1038,7 @@ GTEST_TEST(MultibodyPlantTest, SetDefaultFreeBodyPose) {
   MultibodyPlant<double> plant(0.0);
   // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
   const auto& body = plant.AddRigidBody("body",
-      SpatialInertia<double>::MakeTestCube());
+      SpatialInertia<double>::MakeUnitary());
   EXPECT_TRUE(CompareMatrices(
       plant.GetDefaultFreeBodyPose(body).GetAsMatrix4(),
       RigidTransformd::Identity().GetAsMatrix4()));
@@ -1195,7 +1195,7 @@ class SphereChainScenario {
       const double radius = 0.5;
       // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
       const RigidBody<double>& sphere = plant_->AddRigidBody(
-          "Sphere" + to_string(i), SpatialInertia<double>::MakeTestCube());
+          "Sphere" + to_string(i), SpatialInertia<double>::MakeUnitary());
       GeometryId sphere_id = plant_->RegisterCollisionGeometry(
           sphere, RigidTransformd::Identity(), geometry::Sphere(radius),
           "collision", CoulombFriction<double>());
@@ -1228,7 +1228,7 @@ class SphereChainScenario {
     // Body with no registered frame.
     // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
     no_geometry_body_ = &plant_->AddRigidBody("NothingRegistered",
-        SpatialInertia<double>::MakeTestCube());
+        SpatialInertia<double>::MakeUnitary());
   }
 
   void Finalize() {
@@ -1413,7 +1413,7 @@ GTEST_TEST(MultibodyPlantTest, FilterWeldedSubgraphs) {
 GTEST_TEST(MultibodyPlantTest, CollectRegisteredGeometriesErrors) {
   MultibodyPlant<double> plant(0.0);
   const RigidBody<double>& body = plant.AddRigidBody("body",
-      SpatialInertia<double>::MakeTestCube());
+      SpatialInertia<double>::MakeUnitary());
 
   // It's an error to call this without a SceneGraph.
   DRAKE_EXPECT_THROWS_MESSAGE(
@@ -1628,8 +1628,8 @@ GTEST_TEST(MultibodyPlantTest, CollisionGeometryRegistration) {
 
   // Add two spherical bodies.
   // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
-  const RigidBody<double>& sphere1 =
-      plant.AddRigidBody("Sphere1", SpatialInertia<double>::MakeTestCube());
+  const RigidBody<double>& sphere1 = plant.AddRigidBody("Sphere1",
+          SpatialInertia<double>::MakeUnitary());
   CoulombFriction<double> sphere1_friction(0.8, 0.5);
   // estimated parameters for mass=1kg, penetration_tolerance=0.01m
   // and gravity g=9.8 m/s^2.
@@ -1665,8 +1665,8 @@ GTEST_TEST(MultibodyPlantTest, CollisionGeometryRegistration) {
   sphere2_properties.AddProperty(geometry::internal::kMaterialGroup,
                                  geometry::internal::kHcDissipation,
                                  sphere2_dissipation);
-  const RigidBody<double>& sphere2 =
-      plant.AddRigidBody("Sphere2", SpatialInertia<double>::MakeTestCube());
+  const RigidBody<double>& sphere2 = plant.AddRigidBody("Sphere2",
+      SpatialInertia<double>::MakeUnitary());
   GeometryId sphere2_id = plant.RegisterCollisionGeometry(
       sphere2, RigidTransformd::Identity(), geometry::Sphere(radius),
       "collision", std::move(sphere2_properties));
@@ -1795,15 +1795,15 @@ GTEST_TEST(MultibodyPlantTest, VisualGeometryRegistration) {
 
   // Add two spherical bodies.
   // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
-  const RigidBody<double>& sphere1 =
-      plant.AddRigidBody("Sphere1", SpatialInertia<double>::MakeTestCube());
+  const RigidBody<double>& sphere1 = plant.AddRigidBody("Sphere1",
+      SpatialInertia<double>::MakeUnitary());
   Vector4<double> sphere1_diffuse{0.9, 0.1, 0.1, 0.5};
   GeometryId sphere1_id = plant.RegisterVisualGeometry(
       sphere1, RigidTransformd::Identity(), geometry::Sphere(radius),
       "visual", sphere1_diffuse);
   EXPECT_EQ(render_engine.num_registered(), 2);
-  const RigidBody<double>& sphere2 =
-      plant.AddRigidBody("Sphere2", SpatialInertia<double>::MakeTestCube());
+  const RigidBody<double>& sphere2 = plant.AddRigidBody("Sphere2",
+      SpatialInertia<double>::MakeUnitary());
   IllustrationProperties sphere2_props;
   const Vector4<double> sphere2_diffuse{0.1, 0.9, 0.1, 0.5};
   sphere2_props.AddProperty("phong", "diffuse", sphere2_diffuse);
@@ -2035,8 +2035,8 @@ void InitializePlantAndContextForVelocityToQDotMapping(
     MultibodyPlant<double>* plant, std::unique_ptr<Context<double>>* context) {
   // This is used in purely kinematic tests.
   // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
-  const RigidBody<double>& body =
-      plant->AddRigidBody("FreeBody", SpatialInertia<double>::MakeTestCube());
+  const RigidBody<double>& body = plant->AddRigidBody("FreeBody",
+      SpatialInertia<double>::MakeUnitary());
   plant->Finalize();
 
   *context = plant->CreateDefaultContext();
@@ -2281,15 +2281,15 @@ class MultibodyPlantContactJacobianTests : public ::testing::Test {
 
     // The model simply contains a small and a large box.
     // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
-    const RigidBody<double>& large_box =
-        plant_.AddRigidBody("LargeBox", SpatialInertia<double>::MakeTestCube());
+    const RigidBody<double>& large_box = plant_.AddRigidBody("LargeBox",
+        SpatialInertia<double>::MakeUnitary());
     large_box_id_ = plant_.RegisterCollisionGeometry(
         large_box, RigidTransformd::Identity(),
         geometry::Box(large_box_size_, large_box_size_, large_box_size_),
         "collision", CoulombFriction<double>());
 
-    const RigidBody<double>& small_box =
-        plant_.AddRigidBody("SmallBox", SpatialInertia<double>::MakeTestCube());
+    const RigidBody<double>& small_box = plant_.AddRigidBody("SmallBox",
+        SpatialInertia<double>::MakeUnitary());
     small_box_id_ = plant_.RegisterCollisionGeometry(
         small_box, RigidTransformd::Identity(),
         geometry::Box(small_box_size_, small_box_size_, small_box_size_),
@@ -3341,7 +3341,7 @@ GTEST_TEST(SetRandomTest, FloatingBodies) {
 
   // To avoid unnecessary warnings/errors, use a non-zero spatial inertia.
   const Body<double>& body = plant.AddRigidBody("LoneBody",
-      SpatialInertia<double>::MakeTestCube());
+      SpatialInertia<double>::MakeUnitary());
   plant.Finalize();
 
   RandomGenerator generator;

multibody/tree/spatial_inertia.cc

@@ -8,10 +8,10 @@ namespace drake {
 namespace multibody {
 
 template <typename T>
-SpatialInertia<T> SpatialInertia<T>::MakeTestCube(T mass, T length) {
-  const UnitInertia<T> G_BBcm_B = UnitInertia<T>::SolidCube(length);
-  const Vector3<T> p_BoBcm_B(length / 2, 0, 0);  // Position from Bo to Bcm.
-  const UnitInertia<T> G_BBo_B = G_BBcm_B.ShiftFromCenterOfMass(-p_BoBcm_B);
+SpatialInertia<T> SpatialInertia<T>::MakeUnitary() {
+  const T mass = 1;
+  const Vector3<T> p_BoBcm_B = Vector3<T>::Zero();  // Position from Bo to Bcm.
+  const UnitInertia<T> G_BBo_B(/* Ixx = */ 1, /* Iyy = */ 1, /* Izz = */ 1);
   return SpatialInertia<T>(mass, p_BoBcm_B, G_BBo_B);
 }
 

multibody/tree/spatial_inertia.h

@@ -119,20 +119,10 @@ class SpatialInertia {
     return SpatialInertia(mass, p_PScm_E, G_SP_E);
   }
 
-  /// (Internal use only) Creates a spatial inertia for a uniform-density cube B
-  /// about Bo (B's origin point) with a given mass and length.
-  /// @param[in] mass The mass of the cube.
-  /// @param[in] length The length (or width or depth) of the cube.
-  /// @retval M_BBo_B Cube B's spatial inertia about point Bo (B's origin),
-  /// expressed in terms of unit vectors Bx, By, Bz, each of which are parallel
-  /// to sides of the cube. Point Bo is the centroid of the face of the cube
-  /// whose outward normal is -Bx. The position vector from Bo to Bcm (B's
-  /// center of mass) is p_BoBcm_B = length/2 Bx.
-  /// @throws std::exception if the spatial inertia is invalid, which happens
-  /// if mass is negative or length is negative.
-  /// @note The default parameters mass = 2 and length = 3 correspond to a mass
-  /// moment of inertia of 3 for any line that pass through Bcm.
-  static SpatialInertia<T> MakeTestCube(T mass = T(2), T length = T(3));
+  /// (Internal use only) Creates a spatial inertia whose mass is 1, position
+  /// vector to center of mass is zero, and whose rotational inertia has
+  /// moments of inertia of 1 and products of inertia of 0.
+  static SpatialInertia<T> MakeUnitary();
 
   /// Default SpatialInertia constructor initializes mass, center of mass and
   /// rotational inertia to invalid NaN's for a quick detection of

multibody/tree/test/multibody_tree_creation_test.cc

@@ -832,14 +832,33 @@ GTEST_TEST(WeldedBodies, CreateListOfWeldedBodies) {
   }
 }
 
+// Helper function to create a unit inertia for a uniform-density cube B about
+// Bo (B's origin point) from a given dimension (length).
+// If length = 0, the spatial inertia is that of a particle.
+// @param[in] length The length of any of the cube's edges.
+// @retval M_BBo_B Cube B's unit inertia about point Bo (B's origin),
+// expressed in terms of unit vectors Bx, By, Bz, each of which are parallel
+// to sides (edges) of the cube. Point Bo is the centroid of the face of the
+// cube whose outward normal is -Bx. Hence, the position vector from Bo to Bcm
+// (B's center of mass) is p_BoBcm_B = Lx/2 Bx.
+UnitInertia<double> MakeTestCubeUnitInertia(const double length = 1.0) {
+    const UnitInertia<double> G_BBcm_B = UnitInertia<double>::SolidCube(length);
+    const Vector3<double> p_BoBcm_B(length / 2, 0, 0);
+    const UnitInertia<double> G_BBo_B =
+        G_BBcm_B.ShiftFromCenterOfMass(-p_BoBcm_B);
+    return G_BBo_B;
+}
+
 // Helper function to add a rigid body to a model.
 const RigidBody<double>& AddRigidBody(MultibodyTree<double>* model,
                                       const std::string& name,
                                       const double mass,
                                       const double link_length = 1.0) {
     DRAKE_DEMAND(model != nullptr);
-    return model->AddRigidBody(name,
-        SpatialInertia<double>::MakeTestCube(mass, link_length));
+    const Vector3<double> p_BoBcm_B(link_length / 2, 0, 0);
+    const UnitInertia<double> G_BBo_B = MakeTestCubeUnitInertia(link_length);
+    const SpatialInertia<double> M_BBo_B(mass, p_BoBcm_B, G_BBo_B);
+    return model->AddRigidBody(name, M_BBo_B);
 }
 
 // Verify Body::default_rotational_inertia() and related MultibodyTree methods.
@@ -859,8 +878,7 @@ GTEST_TEST(DefaultInertia, VerifyDefaultRotationalInertia) {
 
   // Verify the default rotational inertia for each of the bodies.
   // To help with testing, create a RotationalInertia for a unit mass cube.
-  const UnitInertia<double> G_SSo_S =
-      SpatialInertia<double>::MakeTestCube(1.0, length).get_unit_inertia();
+  const UnitInertia<double> G_SSo_S = MakeTestCubeUnitInertia(length);
   const RotationalInertia<double> I_A = body_A.default_rotational_inertia();
   const RotationalInertia<double> I_B = body_B.default_rotational_inertia();
   const RotationalInertia<double> I_C = body_C.default_rotational_inertia();

multibody/tree/test/spatial_inertia_test.cc

@@ -53,64 +53,17 @@ GTEST_TEST(SpatialInertia, DefaultConstructor) {
   ASSERT_TRUE(I.IsNaN());
 }
 
-GTEST_TEST(SpatialInertia, MakeTestCubeSpatialInertia) {
-  // Drake allows a spatial inertia to have zero mass and/or zero inertia.
-  const double mass_0 = 0.0;
-  const double length_0 = 0.0;
-  const SpatialInertia<double> M0 =
-      SpatialInertia<double>::MakeTestCube(mass_0, length_0);
-  EXPECT_TRUE(M0.IsPhysicallyValid());
-  EXPECT_EQ(M0.get_mass(), mass_0);
-  EXPECT_EQ(M0.get_com(), Vector3<double>::Zero());
-  const Vector3<double> M0_unit_moments = M0.get_unit_inertia().get_moments();
-  const Vector3<double> M0_unit_products = M0.get_unit_inertia().get_products();
-  EXPECT_EQ(M0_unit_moments, Vector3<double>::Zero());
-  EXPECT_EQ(M0_unit_products, Vector3<double>::Zero());
-
-  const double mass_1 = 1.0;
-  const double length_2 = 2.0;
-  const SpatialInertia<double> M1 =
-      SpatialInertia<double>::MakeTestCube(mass_1, length_2);
-  EXPECT_TRUE(M1.IsPhysicallyValid());
-  EXPECT_EQ(M1.get_mass(), mass_1);
-  EXPECT_TRUE(CompareMatrices(
-      M1.get_com(), Vector3<double>(length_2 / 2, 0, 0), kEpsilon));
-  const Vector3<double> M1_unit_moments = M1.get_unit_inertia().get_moments();
-  const Vector3<double> M1_unit_products = M1.get_unit_inertia().get_products();
-  const double I1 = length_2 * length_2 / 6.0;      // Expected Ixx unit moment.
-  const double J1 = I1 + (length_2 / 2) * (length_2 / 2);  // Expected Iyy, Izz.
-  EXPECT_TRUE(CompareMatrices(
-      M1_unit_moments, Vector3<double>(I1, J1, J1), kEpsilon));
-  EXPECT_EQ(M1_unit_products, Vector3<double>::Zero());
-
-  // Test default mass and length parameters for MakeTestCube().
-  const double default_mass = 2;
-  const double default_length = 3;
-  const SpatialInertia<double> M2 = SpatialInertia<double>::MakeTestCube();
-  EXPECT_TRUE(M2.IsPhysicallyValid());
-  EXPECT_EQ(M2.get_mass(), default_mass);
-  EXPECT_TRUE(CompareMatrices(
-      M2.get_com(), Vector3<double>(default_length / 2, 0, 0), kEpsilon));
-  const Vector3<double> M2_unit_moments = M2.get_unit_inertia().get_moments();
-  const Vector3<double> M2_unit_products = M2.get_unit_inertia().get_products();
-  const double I2 = default_length * default_length / 6.0;
-  const double J2 = I2 + (default_length / 2) * (default_length / 2);
-  EXPECT_TRUE(CompareMatrices(
-      M2_unit_moments, Vector3<double>(I2, J2, J2), kEpsilon));
-  EXPECT_EQ(M2_unit_products, Vector3<double>::Zero());
-
-  // Ensure an exception is thrown if mass is negative.
-  const std::string expected_mass_msg =
-      "Spatial inertia fails SpatialInertia::IsPhysicallyValid\\(\\).\n"
-      "mass = -1(\\.0)? is negative.\n";
-  DRAKE_EXPECT_THROWS_MESSAGE(SpatialInertia<double>::MakeTestCube(
-      -mass_1, length_2), expected_mass_msg);
-
-  // Ensure an exception is thrown if length is negative.
-  const std::string expected_length_msg = "A length argument to "
-                                   "UnitInertia::SolidBox\\(\\) is negative.";
-  DRAKE_EXPECT_THROWS_MESSAGE(SpatialInertia<double>::MakeTestCube(
-      mass_1, -length_2), expected_length_msg);
+GTEST_TEST(SpatialInertia, MakeUnitary) {
+  const double expected_mass = 1;
+  const SpatialInertia<double> M = SpatialInertia<double>::MakeUnitary();
+  EXPECT_TRUE(M.IsPhysicallyValid());
+  EXPECT_EQ(M.get_mass(), expected_mass);
+  EXPECT_EQ(M.get_com(), Vector3<double>::Zero());
+  const Vector3<double> M_unit_moments = M.get_unit_inertia().get_moments();
+  const Vector3<double> M_unit_products = M.get_unit_inertia().get_products();
+  const double Ixx = 1;  // Expected Ixx = Iyy = Izz unit moment of inertia.
+  EXPECT_EQ(M_unit_moments, Vector3<double>(Ixx, Ixx, Ixx));
+  EXPECT_EQ(M_unit_products, Vector3<double>::Zero());
 }
 
 // Test the construction from the mass, center of mass, and unit inertia of a