Change SpatialInertia::MakeTestCube() to MakeUnitary().

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/multibody_tree.cc

@@ -2869,6 +2869,115 @@ void MultibodyTree<T>::ThrowIfNotFinalized(const char* source_method) const {
   }
 }
 
+template<typename T>
+void MultibodyTree<T>::IssuePostFinalizeMassInertiaWarnings() const {
+  ThrowIfNotFinalized(__func__);
+
+  // Get `this` multibody tree's topology and use one of its functions to build
+  // multiple sets of bodies that are welded to each other. Each set of bodies
+  // consists of a parent body (the first entry in the set) and thereafter
+  // children bodies that are welded to the parent body.
+  const MultibodyTreeTopology& topology = get_topology();
+  std::vector<std::set<BodyIndex>> welded_bodies =
+      topology.CreateListOfWeldedBodies();
+
+  // There should be at least 1 set of welded_bodies since the first set should
+  // be the world body (and if it has children bodies, they are anchored to it).
+  const size_t number_of_sets = welded_bodies.size();
+  DRAKE_ASSERT(number_of_sets > 0);
+
+  // Investigate mass/inertia properties for all non-world welded bodies.
+  // In the for-loop below, start with i = 1 to skip over the world body.
+  const MultibodyTreeTopology& multibodyTreeTopology = get_topology();
+  for (size_t i = 1;  i < number_of_sets;  ++i) {
+    // Get the next set of welded bodies. The first entry in the set is the
+    // parent body and the remaining entries (if any) are children bodies.
+    const std::set<BodyIndex>& welded_parent_children_bodies = welded_bodies[i];
+    const BodyIndex parent_body_index = *welded_parent_children_bodies.begin();
+    const BodyTopology& parent_body_topology =
+        multibodyTreeTopology.get_body(parent_body_index);
+    const MobilizerIndex& parent_mobilizer_index =
+        parent_body_topology.inboard_mobilizer;
+    const Mobilizer<T>& parent_mobilizer =
+        get_mobilizer(parent_mobilizer_index);
+
+    // Check previous assumptions.
+    const Body<T>& parent_body = get_body(parent_body_index);
+    DRAKE_DEMAND(parent_body_index == parent_body.index());
+    DRAKE_DEMAND(parent_body_index != world_index());
+
+    // Determine whether this set of welded bodies is the most distal leaf in
+    // a multibody tree.
+    const BodyNodeIndex parent_body_node_index =
+        multibodyTreeTopology.get_body(parent_body_index).body_node;
+    const BodyNodeTopology& parent_body_node_topology =
+        multibodyTreeTopology.get_body_node(parent_body_node_index);
+    const bool is_composite_body_distal_leaf_in_tree =
+        multibodyTreeTopology.CalcNumberOfOutboardVelocitiesExcludeBase(
+            parent_body_node_topology) == 0;
+    if (is_composite_body_distal_leaf_in_tree) {
+      // Determine if this distal-leaf composite body can translate relative to
+      // its inboard object but has no mass.
+      const bool has_no_mass =
+          CalcTotalDefaultMass(welded_parent_children_bodies) == 0;
+      if (parent_mobilizer.can_translate() && has_no_mass) {
+         const std::string msg = fmt::format(
+        "It seems that body {} is massless, yet it is attached "
+        "by a joint that has a translational degree of freedom.",
+        parent_body.name());
+        throw std::logic_error(msg);
+      }
+
+      // Issue error if distal composite body can rotate and has no inertia.
+      // TODO(Mitiguy) For now, the algorithm below issues a message only if
+      //  there is no rotational inertia and no mass that could be shfted to
+      //  create non-zero moment of inertia about a joint axes. Instead, form
+      //  rotational inertia about mobilizer axes and check if it is zero.
+      const bool can_rotate_but_no_inertia = parent_mobilizer.can_rotate() &&
+        IsAllDefaultRotationalInertiaZeroOrNaN(welded_parent_children_bodies);
+      if (can_rotate_but_no_inertia && has_no_mass) {
+    // Issue zero rotational inertia if the composite rigid body can rotate.
+    if (parent_mobilizer.can_rotate()) {
+      const bool is_rotational_inertia_nonzero =
+        IsTotalDefaultRotationalInertiaNonZero(welded_parent_children_bodies);
+      if (!is_rotational_inertia_nonzero) {
+        const std::string msg = fmt::format(
+            "It seems that body {} has no rotational inertia, yet it is "
+            "attached by a joint that has a rotational degree of freedom.",
+            parent_body.name());
+        "It seems that body {} has no rotational inertia, yet it is attached "
+        "by a joint that has a rotational degree of freedom.",
+        parent_body_name);
+        throw std::logic_error(msg);
+      }
+    }
+  }
+}
+
+template <typename T>
+double MultibodyTree<T>::CalcTotalDefaultMass(
+    const std::set<BodyIndex>& body_indexes) const {
+  double total_mass = 0;
+  for (BodyIndex body_index : body_indexes) {
+    const Body<T>& body_B = get_body(body_index);
+    const double mass_B = body_B.get_default_mass();
+    if (!std::isnan(mass_B)) total_mass += mass_B;
+  }
+  return total_mass;
+}
+
+template <typename T>
+bool MultibodyTree<T>::IsTotalDefaultRotationalInertiaNonZero(
+      const std::set<BodyIndex>& body_indexes) const {
+  for (BodyIndex body_index : body_indexes) {
+    const Body<T>& body_B = get_body(body_index);
+    const RotationalInertia<double> I_BBo_B =
+        body_B.default_rotational_inertia();
+    if (!I_BBo_B.IsNaN() && !I_BBo_B.IsZero()) return true;
+  }
+  return false;  // Total default rotational inertia is NaN or zero.
+}
+
 template <typename T>
 double MultibodyTree<T>::CalcTotalDefaultMass(
     const std::set<BodyIndex>& body_indexes) const {

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