Add new test that fails

multibody/topology/link_joint_graph.cc

@@ -471,6 +471,16 @@ std::vector<std::set<LinkIndex>> LinkJointGraph::FindSubgraphsOfWeldedLinks()
   return subgraphs;
 }
 
+#ifdef NOTDEF
+// Our composite_links collection doesn't include lone Links that aren't welded
+// to anything. The return from this function must include every Link, with
+// the World link in the first set (even if nothing is welded to it).
+std::vector<std::set<LinkIndex>> LinkJointGraph::FindSubgraphsOfWeldedLinks()
+const {
+
+}
+#endif
+
 // As mentioned above, the use of "parent" here does not imply anything about
 // the Parent/Child Joint connection. It is really just the root of a subgraph.
 // TODO(sherm1) Reimplement using already-built Composites.

multibody/topology/link_joint_graph.h

@@ -315,6 +315,10 @@ class LinkJointGraph {
   const std::vector<std::vector<LinkIndex>>& composite_links() const {
     return data_.composite_links;
   }
+  const std::vector<LinkIndex>& composite_links(
+      CompositeLinkIndex composite_link_index) const {
+    return composite_links().at(composite_link_index);
+  }
 
   /** @returns `true` if a Link named `name` was added to `model_instance`.
   @see AddLink().
@@ -366,6 +370,7 @@ class LinkJointGraph {
       element zero in the returned vector.
     - The minimum number of subgraphs is one. This corresponds to a graph with
       all Links welded to the world. */
+  std::vector<std::set<LinkIndex>> XFindSubgraphsOfWeldedLinks() const;
   std::vector<std::set<LinkIndex>> FindSubgraphsOfWeldedLinks() const;
 
   /** Returns all Links that are transitively welded, or rigidly affixed, to

multibody/topology/spanning_forest.cc

@@ -341,7 +341,7 @@ void SpanningForest::ExtendTreesOneLevel(
     const LinkIndex inboard_link_index =
         parent_is_modeled ? joint.parent_link() : joint.child_link();
 
-    // Don't keep references to mobods since we're growing the vector below.
+    // Don't keep references to Mobods since we're growing the vector below.
     const MobodIndex inboard_mobod_index =
         graph().link_to_mobod(inboard_link_index);
 

multibody/topology/test/link_joint_graph_test.cc

@@ -20,9 +20,9 @@ const char kPrismaticType[] = "prismatic";
 // Arbitrary world name for testing.
 const char kWorldLinkName[] = "DefaultWorldLinkName";
 
-// Test a straightforward serial chain of bodies connected by
-// revolute joints: world->body1->body2->body3->body4->body5.
-// We perform a number of sanity checks on the provided API.
+/* Test a straightforward serial chain of bodies connected by
+revolute joints: world->body1->body2->body3->body4->body5.
+We perform a number of sanity checks on the provided API. */
 GTEST_TEST(LinkJointGraph, SerialChain) {
   LinkJointGraph graph;
   EXPECT_EQ(graph.num_joint_types(), 3);  // predefined types thus far
@@ -1107,6 +1107,69 @@ GTEST_TEST(LinkJointGraph, LoopWithComposites) {
   graph_copy.forest().SanityCheckForest();  // Should be empty but OK
 }
 
+/* For both composite_links and composite_mobods: if there is anything welded
+to World, that composite must come first. This graph's first branch has
+a composite that could be seen prior to the weld to World. We'll attempt
+to trick it into following that path by using a massless body, requiring it
+to extend the first branch to Link {2} before moving on to the next branch.
+
+          +---> {1*} ===> {2}
+      {0} | 0         1                {Links} & Joints
+    World |                            ===> is a weld
+          +===> {3}                    * Link 1 is massless
+          | 2
+          |
+          +---> {4}
+            3
+*/
+
+GTEST_TEST(LinkJointGraph, WorldCompositeComesFirst) {
+  LinkJointGraph graph;
+  graph.RegisterJointType(kRevoluteType, 1, 1);
+  const ModelInstanceIndex model_instance(5);  // arbitrary
+
+  // The first Link added defines the world's name and forest instance.
+  graph.AddLink("world", world_model_instance());
+  graph.AddLink("massless_link_1", model_instance, LinkFlags::TreatAsMassless);
+  graph.AddLink("link2", model_instance);
+  graph.AddLink("link3", model_instance);
+  graph.AddLink("link4", model_instance);
+
+  const auto& world = graph.links(LinkIndex(0));
+  const auto& massless_link = graph.links(LinkIndex(1));
+  const auto& link2 = graph.links(LinkIndex(2));
+  const auto& link3 = graph.links(LinkIndex(3));
+  const auto& link4 = graph.links(LinkIndex(4));
+
+  graph.AddJoint("joint0", model_instance,
+                 kRevoluteType, world.index(), massless_link.index());
+  graph.AddJoint("joint1", model_instance,
+                 "weld", massless_link.index(), link2.index());
+  graph.AddJoint("joint2", model_instance,
+                 "weld", world.index(), link3.index());
+  graph.AddJoint("joint4", model_instance,
+                 kRevoluteType, world.index(), link4.index());
+
+  const SpanningForest& forest = graph.BuildForest();  // Default options
+  graph.DumpGraph("WorldCompositeComesFirst");
+  forest.DumpForest("WorldCompositeComesFirst");
+
+  EXPECT_EQ(ssize(graph.links()), 5);
+  EXPECT_EQ(ssize(forest.mobods()), 5);  // Because we're not combining.
+
+  // "Anchored" means "fixed to World" (by welds).
+  EXPECT_TRUE(world.is_anchored());
+  EXPECT_FALSE(massless_link.is_anchored());
+  EXPECT_FALSE(link2.is_anchored());
+  EXPECT_TRUE(link3.is_anchored());
+  EXPECT_FALSE(link4.is_anchored());
+
+  EXPECT_EQ(ssize(graph.composite_links()), 2);
+  EXPECT_EQ(ssize(forest.composite_mobods()), 2);
+
+  EXPECT_EQ(graph.composite_links(CompositeLinkIndex(0))[0], world.index());
+}
+
 }  // namespace internal
 }  // namespace multibody
 }  // namespace drake