Fix composite body code so more tests can pass.

mitiguy · mitiguy · commit f82318fcc102 · 2026-06-30T19:48:36.000-07:00
diff --git a/multibody/plant/test/composite_test.cc b/multibody/plant/test/composite_test.cc
@@ -124,10 +124,14 @@ TestModel MakeModel(bool combine_welded_bodies) {
   // Sanity check: Some information in the SpanningForest should be the same,
   // whether or not the welded links are combined.
   const internal::SpanningForest& forest = tree.forest();
+  const internal::SpanningForest::Mobod& mobod_0 =
+      forest.mobods(internal::MobodIndex(0));
   const internal::SpanningForest::Mobod& mobod_1 =
       forest.mobods(internal::MobodIndex(1));
-  EXPECT_EQ(mobod_1.active_link_ordinal(), LinkOrdinal(1));
+  EXPECT_TRUE(mobod_0.is_world());      // World is mobod(0) and LinkOrdinal(0).
   EXPECT_TRUE(mobod_1.is_base_body());  // Connects to World via revolute joint.
+  EXPECT_EQ(mobod_0.active_link_ordinal(), LinkOrdinal(0));
+  EXPECT_EQ(mobod_1.active_link_ordinal(), LinkOrdinal(1));
 
   // If the welded links are combined, verify that the composite mobilized body
   // has the proper follower link ordinals (the active link ordinal which
@@ -175,96 +179,68 @@ GTEST_TEST(CompositeTest, CompositeSpatialInertia) {
   const TestModel explicit_model = MakeModel(false /* no combining */);
   const TestModel composite_model = MakeModel(true /* combine welds */);
 
+  const double m = 1.0, a = 0.1;  // mass m and side-length a of solid cubes.
+  const Frame<double>& world_frame = explicit_model.plant->world_frame();
+  const RigidBody<double>* explicit_links[] = {
+      explicit_model.link1, explicit_model.link2, explicit_model.link3,
+      explicit_model.link4};
+  const RigidBody<double>* composite_links[] = {
+      composite_model.link1, composite_model.link2, composite_model.link3,
+      composite_model.link4};
+
   // The mass matrix is configuration-independent for this model (see above),
   // but we check at several angles to guard against future changes.
   const std::vector<double> angles = {0.0, M_PI / 6, M_PI / 4, -M_PI / 3};
   for (double angle : angles) {
     SetState(explicit_model, angle, 0.0);
     SetState(composite_model, angle, 0.0);
 
-    // Verify Link123's spatial inertia not depend on combined welded links
-    const Frame<double>& world_frame = explicit_model.plant->world_frame();
+    // Verify Link123's spatial inertia not depend on combined welded links.
+    // Note: Due to Bug 1 above, the following test used to fail (now fixed)
     SpatialInertia<double> M_EWo_W = explicit_model.plant->CalcSpatialInertia(
         *explicit_model.context, world_frame,
         {explicit_model.link1->index(), explicit_model.link2->index(),
          explicit_model.link3->index()});
-
-    // TODO(Mitiguy) This next tests should be EXPECT_TRUE, hence a bug in 
-    // MultibodyPlant::CalcSpatialInertia(), maybe something like poses 
-    // using MoBod index rather linkIndex or cache not updated or ...         
     SpatialInertia<double> M_CWo_W = composite_model.plant->CalcSpatialInertia(
         *composite_model.context, world_frame,
         {composite_model.link1->index(), composite_model.link2->index(),
-         composite_model.link3->index()});        
-    EXPECT_FALSE(CompareMatrices(M_EWo_W.CopyToFullMatrix6(),
+         composite_model.link3->index()});
+    EXPECT_TRUE(CompareMatrices(M_EWo_W.CopyToFullMatrix6(),
                                 M_CWo_W.CopyToFullMatrix6(), kTolerance,
                                 MatrixCompareType::relative))
-        << "Spatial inertia mismatch at angle = " << angle;
-
-    // TODO(Mitiguy) The next three tests show that there is a bug in
-    // MultibodyPlant::CalcSpatialInertia() that incorrectly associates the
-    // Link123 composite spatial inertia with a single link (Link 1 or 2 or 3).
-    M_CWo_W = composite_model.plant->CalcSpatialInertia(
-        *composite_model.context, world_frame,
-        {composite_model.link1->index()});
-    EXPECT_FALSE(CompareMatrices(M_EWo_W.CopyToFullMatrix6(),
-                                 M_CWo_W.CopyToFullMatrix6(), kTolerance,
-                                 MatrixCompareType::relative))
-        << "Spatial inertia mismatch at angle = " << angle;
-
-    // TODO(Mitiguy) This test and the previous and next test should NOT pass.
-    M_CWo_W = composite_model.plant->CalcSpatialInertia(
-        *composite_model.context, world_frame,
-        {composite_model.link2->index()});
-    EXPECT_FALSE(CompareMatrices(M_EWo_W.CopyToFullMatrix6(),
-                                 M_CWo_W.CopyToFullMatrix6(), kTolerance,
-                                 MatrixCompareType::relative))
-        << "Spatial inertia mismatch at angle = " << angle;
-
-    // TODO(Mitiguy) This test and the previous two test should NOT pass.
-    M_CWo_W = composite_model.plant->CalcSpatialInertia(
-        *composite_model.context, world_frame,
-        {composite_model.link3->index()});
-    EXPECT_FALSE(CompareMatrices(M_EWo_W.CopyToFullMatrix6(),
-                                 M_CWo_W.CopyToFullMatrix6(), kTolerance,
-                                 MatrixCompareType::relative))
-        << "Spatial inertia mismatch at angle = " << angle;
-
-    // Calculate Link4's spatial inertia about Wo (world origin) expressed in W.
-    const double m = 1.0, a = 0.1;  // Cube's mass (kg) and side-length (meter).
-    const Vector3<double> p_WoL4o_W(4.0, 0.0, 0.0);
-    SpatialInertia<double> M_L4Wo_W_expected =
-        SpatialInertia<double>::SolidCubeWithMass(m, a).Shift(-p_WoL4o_W);
-
-    // Ensure the spatial inertia for Link 4 is correct in the explicit model.
-    SpatialInertia<double> M_L4Wo_W = explicit_model.plant->CalcSpatialInertia(
-        *explicit_model.context, world_frame, {explicit_model.link4->index()});
-    EXPECT_TRUE(CompareMatrices(M_L4Wo_W.CopyToFullMatrix6(),
-                                M_L4Wo_W_expected.CopyToFullMatrix6(),
-                                kTolerance, MatrixCompareType::relative))
-        << "Spatial inertia mismatch for Link4 at angle = " << angle;
-
-    // Ensure the spatial inertia for Link 4 is correct in the composite model.
-    // TODO(Mitiguy) There is a bug in MultibodyPlant::CalcSpatialInertia() when
-    // composite mobilized bodies are used. The spatial inertia for the World
-    // body (which is nan) replaces that of Link 4 (which is welded to world).
-    // TODO(Mitiguy) Remove dead code after we are sure the bug is fixed.    
-    if (!kDrakeAssertIsArmed) { 
-      M_L4Wo_W = composite_model.plant->CalcSpatialInertia(
-          *composite_model.context, world_frame,
-          {composite_model.link4->index()});
-      // TODO(Mitiguy) This test should be EXPECT_TRUE not EXPECT_FALSE.
-      EXPECT_TRUE(CompareMatrices(M_L4Wo_W.CopyToFullMatrix6(),
-                                  M_L4Wo_W_expected.CopyToFullMatrix6(),
-                                  kTolerance, MatrixCompareType::relative))
-          << "Spatial inertia mismatch for Link4 at angle = " << angle;
-    } else {
-      // TODO(Mitiguy) Fix as an exception should not be thrown here.
-      DRAKE_EXPECT_THROWS_MESSAGE_IF_ARMED(
-          composite_model.plant->CalcSpatialInertia(
-              *composite_model.context, world_frame,
-              {composite_model.link4->index()}),
-          "[\\s\\S]*did not pass the test CouldBePhysicallyValid[\\s\\S]*");
+        << "Link123 spatial inertia mismatch at angle = " << angle;
+
+    // Tests that uncovered old bugs in MultibodyPlant::CalcSpatialInertia().
+    // Bug 1: When CalcSpatialInertia() was called with composite_model on a
+    // single link (Link 1 or 2 or 3), the returned spatial inertia was for the
+    // entire Link123 composite mobod.
+    // Bug 2: When CalcSpatialInertia() was called with composite_model on link4
+    // (which is welded to world), an exception was thrown (debug mode) or NaN
+    // were returned (release mode).
+    for (int i = 0; i < 3; ++i) {
+      const RigidBody<double>* explicit_linki = explicit_links[i];
+      const RigidBody<double>* composite_linki = composite_links[i];
+      M_EWo_W = explicit_model.plant->CalcSpatialInertia(
+          *explicit_model.context, world_frame, {explicit_linki->index()});
+      M_CWo_W = composite_model.plant->CalcSpatialInertia(
+          *composite_model.context, world_frame, {composite_linki->index()});
+      EXPECT_TRUE(CompareMatrices(M_EWo_W.CopyToFullMatrix6(),
+                                  M_CWo_W.CopyToFullMatrix6(), kTolerance,
+                                  MatrixCompareType::relative))
+          << "Spatial inertia mismatch: link" << i + 1
+          << " at angle = " << angle;
+
+      // Due to special case of link4 being welded to world, compare link4's
+      // spatial inertia to its expected value.
+      if (i == 3) {
+        const Vector3<double> p_WoL4o_W(4.0, 0.0, 0.0);
+        SpatialInertia<double> M_L4Wo_W_expected =
+            SpatialInertia<double>::SolidCubeWithMass(m, a).Shift(-p_WoL4o_W);
+        EXPECT_TRUE(CompareMatrices(M_L4Wo_W_expected.CopyToFullMatrix6(),
+                                    M_CWo_W.CopyToFullMatrix6(), kTolerance,
+                                    MatrixCompareType::relative))
+            << "Inaccurate link4 spatial inertia at angle = " << angle;
+      }
     }
 
     // Ensure the mass matrix does not depend on welded links being combined.
diff --git a/multibody/tree/body_node_impl.cc b/multibody/tree/body_node_impl.cc
@@ -113,9 +113,31 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcPositionKinematicsCache_BaseToTip(
       break;
   }
 
+#if 0
   // TODO(sherm1) Calculate X_WL for composites. Currently we don't make
   //  composites so X_WL = X_WB if L is the link on B.
   pc->SetX_WL(mobilizer_->mobod().active_link_ordinal(), X_WB);
+#endif
+
+  // Set X_WL for the active (most-inboard) link of this mobod. Its link frame
+  // L coincides with the mobod frame B (X_BL = Identity), so X_WL = X_WB.
+  const SpanningForest::Mobod& mobod = mobilizer_->mobod();
+  pc->SetX_WL(mobod.active_link_ordinal(), X_WB);
+
+  // For composite mobods, also set X_WL for each follower (non-active) link Lₒ.
+  // Because Lₒ is rigidly offset from B by X_BLₒ (pre-computed in
+  // frame_body_pose_cache during CalcFrameBodyPoses), its world pose is simply:
+  //   X_WLₒ = X_WB * X_BLₒ
+  if (mobod.is_composite()) {
+    const auto& followers = mobod.follower_link_ordinals();
+    // followers[0] is the active link (already handled above); start at 1.
+    for (int i = 1; i < ssize(followers); ++i) {
+      const LinkOrdinal follower_ordinal = followers[i];
+      const math::RigidTransform<T>& X_BL =
+          frame_body_pose_cache.get_X_BL(follower_ordinal);
+      pc->SetX_WL(follower_ordinal, X_WB * X_BL);
+    }
+  }
 
   // Compute shift vector p_PoBo_W from the parent origin to the body origin.
   const Vector3<T>& p_PoBo_P = X_PB.translation();
diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc
@@ -1714,19 +1714,21 @@ void MultibodyTree<T>::CalcFrameBodyPoses(
     frame_body_poses->SetX_BF(frame->index(), X_BL * X_LF);
   }
 
-  // For every mobilized body, precalculate its body-frame spatial inertia
-  // M_BBo_B by shifting and accumulating the spatial inertia M_LLo_L of each
-  // of the links it mobilizes (we call those the mobod's "follower links").
-  // For a non-composite mobod there is only one follower link; for a composite
-  // there are multiple links welded together.
+  // Pass 4 (over all mobods): For every mobod, precalculate its body-frame
+  // spatial inertia M_BBo_B by shifting and accumulating the spatial inertia
+  // M_LLo_L of each of its "follower" links. For a non-composite mobod there is
+  // only one follower link. For a composite, multiple follower links are welded
+  // together as a rigid unit.
   // We also calculate and save p_BoLcm_B, the offset from mobod origin Bo to
   // each of its follower links' centers of mass, expressed in B.
   for (const SpanningForest::Mobod& mobod : forest().mobods()) {
-    if (mobod.is_world()) continue;
-
-    // Zero out the Mobod's mass properties. We'll build these up by
-    // accumulating contributions from all follower links.
-    frame_body_poses->SetM_BBo_B(mobod.index(), SpatialInertia<T>::Zero());
+    // Initialize each non-world Mobod's mass properties to zero and then
+    // accumulate mass contributions from all of its follower links.
+    // Note: The world mobod's spatial inertia is intentionally left as NaN.
+    const bool is_world_mobod = mobod.is_world();
+    if (!is_world_mobod) {
+      frame_body_poses->SetM_BBo_B(mobod.index(), SpatialInertia<T>::Zero());
+    }
 
     // Accumulate the spatial inertia from each link following this mobod.
     for (const LinkOrdinal& link_ordinal : mobod.follower_link_ordinals()) {
@@ -1736,21 +1738,26 @@ void MultibodyTree<T>::CalcFrameBodyPoses(
           link.CalcSpatialInertiaInBodyFrame(context);
       frame_body_poses->SetM_LLo_L(link_ordinal, M_LLo_L);
 
+      // Set p_BoLcm_B and accumulate M_LLo_L into M_BBo_B (unless B is world).
       const Vector3<T>& p_LoLcm_L = M_LLo_L.get_com();
       if (!frame_body_poses->is_X_BL_identity(link_ordinal)) {
         const RigidTransform<T>& X_BL =
             frame_body_poses->get_X_BL(link_ordinal);
         const math::RotationMatrix<T>& R_BL = X_BL.rotation();
         const Vector3<T>& p_BoLo_B = X_BL.translation();
-        const SpatialInertia<T> M_LLo_B = M_LLo_L.ReExpress(R_BL);
-        const SpatialInertia<T> M_LBo_B = M_LLo_B.Shift(-p_BoLo_B);
-        frame_body_poses->AddToM_BBo_B(mobod.index(), M_LBo_B);
         const Vector3<T> p_BoLcm_B = X_BL * p_LoLcm_L;
         frame_body_poses->Set_p_BoLcm_B(link_ordinal, p_BoLcm_B);
+        if (!is_world_mobod) {
+          const SpatialInertia<T> M_LLo_B = M_LLo_L.ReExpress(R_BL);
+          const SpatialInertia<T> M_LBo_B = M_LLo_B.Shift(-p_BoLo_B);
+          frame_body_poses->AddToM_BBo_B(mobod.index(), M_LBo_B);
+        }
       } else {
-        // X_BL is identity.
-        frame_body_poses->AddToM_BBo_B(mobod.index(), M_LLo_L);
+        // X_BL is identity, so p_BoLcm_B = p_LoLcm_L and M_LBo_B = M_LLo_L.
         frame_body_poses->Set_p_BoLcm_B(link_ordinal, p_LoLcm_L);
+        if (!is_world_mobod) {
+          frame_body_poses->AddToM_BBo_B(mobod.index(), M_LLo_L);
+        }
       }
     }
   }
@@ -2549,9 +2556,7 @@ SpatialInertia<T> MultibodyTree<T>::CalcSpatialInertia(
 
   // Add each body's spatial inertia in the world frame W to this system
   // S's spatial inertia in W about Wo (the origin of W), expressed in W.
-  // TODO(Mitiguy) Create SpatialInertia<T>::Zero() and use it below.
-  SpatialInertia<T> M_SWo_W(0., Vector3<T>::Zero(),
-                            UnitInertia<T>::TriaxiallySymmetric(0));
+  SpatialInertia<T> M_SWo_W = SpatialInertia<T>::Zero();
 
   for (BodyIndex body_index : body_indexes) {
     if (body_index == world_index()) continue;  // World inertia does not add.
