Fixed composites to always begin with World

Author: sherm1

multibody/topology/link_joint_graph.cc

@@ -393,6 +393,14 @@ CompositeLinkIndex LinkJointGraph::AddToCompositeLink(
   return existing_composite;
 }
 
+void LinkJointGraph::CreateWorldCompositeLink() {
+  DRAKE_DEMAND(composite_links().empty() && !links().empty());
+  Link& world_link = data_.links[LinkIndex(0)];
+  DRAKE_DEMAND(!world_link.composite_link_index_.is_valid());
+  data_.composite_links.emplace_back(std::vector{LinkIndex(0)});
+  world_link.composite_link_index_ = CompositeLinkIndex(0);
+}
+
 void LinkJointGraph::AddUnmodeledJointToComposite(
     JointIndex unmodeled_joint_index, CompositeLinkIndex composite_link_index) {
   Joint& joint = data_.joints[unmodeled_joint_index];
@@ -471,6 +479,15 @@ 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

@@ -307,14 +307,22 @@ class LinkJointGraph {
     return data_.must_be_base_body_links;
   }
 
-  /** After the Forest has been built, returns groups of Links that are welded
-  together. Each group may be modeled with a single mobilized body or multiple
-  mobilized bodies depending on modeling options. The first entry in each group
-  is the "representative link". Composites are present here only for groups of
-  two or more welded Links. */
+  /** After the SpanningForest has been built, returns groups of Links that are
+  welded together. Each group may be modeled in the Forest with a single
+  mobilized body or multiple mobilized bodies depending on modeling options,
+  but that doesn't change anything here. The first entry in each group is the
+  "representative link". The 0th Composite Link is always present and its first
+  entry is World (Link 0), even if nothing else is welded to World. Otherwise,
+  composites are present here only for groups of two or more welded Links.
+  Composite Links are discovered as a side effect of Forest-building; there is
+  no cost to accessing them here. */
   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 +374,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
@@ -441,6 +450,10 @@ class LinkJointGraph {
   CompositeLinkIndex AddToCompositeLink(LinkIndex existing_link_index,
                                         LinkIndex new_link_index);
 
+  // The World Link must already be in the graph but there are no Composite
+  // Links yet. This creates the 0th Composite Link and puts World in it.
+  void CreateWorldCompositeLink();
+
   // Notes that we didn't model this Joint in the Forest because it is just a
   // weld to an existing Composite.
   void AddUnmodeledJointToComposite(JointIndex unmodeled_joint_index,

multibody/topology/spanning_forest.cc

@@ -71,10 +71,14 @@ void SpanningForest::BuildForest(
 
   SetBaseBodyChoicePolicy();
 
-  // Model the World (Link 0) with mobilized body 0.
+  // Model the World (Link 0) with mobilized body 0. This also starts the
+  // 0th Composite Link in the graph and the 0th Composite Mobod in the Forest.
   data_.mobods.emplace_back(MobodIndex(0), LinkIndex(0));
+  data_.composite_mobods.emplace_back(std::vector{MobodIndex(0)});
+  data_.mobods[MobodIndex(0)].composite_mobod_index_ = CompositeMobodIndex(0);
   mutable_graph().set_primary_mobod_for_link(LinkIndex(0), MobodIndex(0),
                                              JointIndex{});
+  mutable_graph().CreateWorldCompositeLink();
 
   /* Decides on forward/reverse mobilizers; combines composite bodies; chooses
   base bodies and adds 6dof mobilizers for them; splits loops; adds shadow
@@ -341,7 +345,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/spanning_forest.h

@@ -203,21 +203,26 @@ class SpanningForest {
   const Tree& trees(TreeIndex tree_index) const { return trees()[tree_index]; }
 
   /** Groups of mobilized bodies that are connected by Weld mobilizers so have
-  no relative degrees of freedom.  Note that if we are modeling welded-together
-  Links as single Composite Links then each composite gets only a single Mobod
-  and hence there won't be any composite Mobods here. Use mobod_to_links() to
-  find all the Links following a single Mobod.
-
-  If anything is welded to World, that Composite Mobod comes first and contains
-  World, mobilized bodies representing Links marked "Static", and bodies (if
-  any) welded to Static bodies or World (recursively). Those are the "anchored"
-  mobilized bodies. The other groups represent sets of welded-together mobilized
-  bodies. Mobods that are not welded to any other Mobods do not appear here.
+  no relative degrees of freedom. Note that if we are combining welded-together
+  Links into single bodies, then each Composite Link gets only a single Mobod
+  and hence there won't be any composite Mobods here except for World which is
+  always considered a composite Mobod even if nothing is welded to it. Use
+  mobod_to_links() to find all the Links following a single Mobod.
+
+  The World Composite Mobod comes first and contains World, mobilized bodies
+  representing Links marked "Static", and bodies (if any) welded to Static
+  bodies or World (recursively). Those are the "anchored" mobilized bodies. The
+  other groups represent sets of welded-together mobilized bodies. Except for
+  World, Mobods that are not welded to any other Mobods do not appear here.
 
   @see mobod_to_links() */
   const std::vector<std::vector<MobodIndex>>& composite_mobods() const {
     return data_.composite_mobods;
   }
+  const std::vector<MobodIndex>& composite_mobods(
+      CompositeMobodIndex index) const {
+    return composite_mobods()[index];
+  }
 
   /** Returns the global ModelingOptions that were used when this model was last
   built with BuildForest(). You can also provide a ModelInstanceIndex to see

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
@@ -191,7 +191,7 @@ GTEST_TEST(LinkJointGraph, SerialChain) {
      2 [7]    =>free9            (added 6dof, free bodies are always last)
 
    Composite Links: {0 7 6 8} {11 10}
-   Composite Mobods: none
+   Composite Mobods: [0]  (just the World "composite")
    */
 
   // TODO(sherm1) Move to its own test suite.
@@ -250,7 +250,7 @@ GTEST_TEST(LinkJointGraph, SerialChain) {
 
   EXPECT_EQ(ssize(forest.mobods()), 8);
   EXPECT_EQ(ssize(forest.trees()), 3);
-  EXPECT_TRUE(forest.composite_mobods().empty());
+  EXPECT_EQ(ssize(forest.composite_mobods()), 1);  // just World
 
   // Now force one of the joints in the composite to be modeled (meaning it
   // should get its own Mobod). This should split the World composite into
@@ -266,9 +266,9 @@ GTEST_TEST(LinkJointGraph, SerialChain) {
 
   EXPECT_EQ(ssize(forest.mobods()), 9);
   EXPECT_EQ(ssize(forest.trees()), 3);
-  EXPECT_EQ(ssize(forest.composite_mobods()), 1);
+  EXPECT_EQ(ssize(forest.composite_mobods()), 2);
   const std::vector<MobodIndex> now_expected{MobodIndex(6), MobodIndex(7)};
-  EXPECT_EQ(forest.composite_mobods()[0], now_expected);
+  EXPECT_EQ(forest.composite_mobods(CompositeMobodIndex(1)), now_expected);
 
   /* Build again but with the FixedBase option for model_instance, and allow
   joint_10_11 to be part of a composite (default behavior). Now we expect
@@ -278,7 +278,7 @@ GTEST_TEST(LinkJointGraph, SerialChain) {
     0  [1-5]  ->link1->link2->link3->link4->link5
 
   Composite Links: {0 7 6 8 11 10 9}
-  Composite Mobods: none
+  Composite Mobods: [0]  (just World)
   */
   graph.change_joint_flags(joint_10_11_index, JointFlags::Default);
   graph.BuildForest(ModelingOptions::CombineCompositeLinks,
@@ -292,12 +292,13 @@ GTEST_TEST(LinkJointGraph, SerialChain) {
 
   EXPECT_EQ(ssize(forest.mobods()), 6);
   EXPECT_EQ(ssize(forest.trees()), 1);
-  EXPECT_EQ(ssize(forest.composite_mobods()), 0);
+  EXPECT_EQ(ssize(forest.composite_mobods()), 1);  // just World
   const std::vector<LinkIndex> expected_composite_link{
     LinkIndex(0), LinkIndex(7), LinkIndex(6), LinkIndex(8),
     LinkIndex(11), LinkIndex(10), LinkIndex(9)};
   EXPECT_EQ(ssize(graph.composite_links()), 1);
-  EXPECT_EQ(graph.composite_links()[0], expected_composite_link);
+  EXPECT_EQ(graph.composite_links(CompositeLinkIndex(0)),
+            expected_composite_link);
 }
 
 /* This is a straightforward graph with two trees each with multiple branches.
@@ -354,11 +355,11 @@ GTEST_TEST(LinkJointGraph, MultipleBranches) {
   EXPECT_EQ(ssize(graph.links()), 15);  // no links added
   EXPECT_EQ(graph.num_user_joints(), 12);  // the ones added above
   EXPECT_EQ(ssize(graph.joints()), 14);  // modeling adds two floating joints
-  EXPECT_TRUE(graph.composite_links().empty());
+  EXPECT_EQ(ssize(graph.composite_links()), 1);  // just World
 
   EXPECT_EQ(ssize(forest.trees()), 2);
   EXPECT_EQ(ssize(forest.mobods()), 15);  // includes World
-  EXPECT_TRUE(forest.composite_mobods().empty());
+  EXPECT_EQ(ssize(forest.composite_mobods()), 1);  // just World
   EXPECT_EQ(forest.num_positions(), 26);  // 12 revolute, 2 x 17 quat floating
   EXPECT_EQ(forest.num_velocities(), 24);
 
@@ -1037,12 +1038,12 @@ GTEST_TEST(LinkJointGraph, LoopWithComposites) {
   EXPECT_EQ(ssize(graph.links()), 12);  // split one, added shadow
   EXPECT_EQ(ssize(graph.joints()), 11);  // no change
   EXPECT_EQ(ssize(graph.constraints()), 1);  // welded shadow to primary
-  EXPECT_EQ(ssize(graph.composite_links()), 3);
+  EXPECT_EQ(ssize(graph.composite_links()), 4);  // World + 3
 
   EXPECT_EQ(ssize(forest.mobods()), 9);
   EXPECT_EQ(ssize(forest.loop_constraints()), 1);
   EXPECT_EQ(ssize(forest.trees()), 2);
-  EXPECT_EQ(ssize(forest.composite_mobods()), 0);
+  EXPECT_EQ(ssize(forest.composite_mobods()), 1);  // just World
 
   EXPECT_EQ(forest.mobods(MobodIndex(1)).follower_links(),
             (std::vector<LinkIndex>{LinkIndex(1), LinkIndex(2)}));
@@ -1107,6 +1108,92 @@ GTEST_TEST(LinkJointGraph, LoopWithComposites) {
   graph_copy.forest().SanityCheckForest();  // Should be empty but OK
 }
 
+/* For both composite_links and composite_mobods: the World composite must
+come first (even if nothing is welded to World). 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.
+But we want to see the {0,3} composite before the {1,2} composite.
+
+          +---> {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
+  forest.SanityCheckForest();
+  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(graph.composite_links(CompositeLinkIndex(0)),
+            (std::vector<LinkIndex>{LinkIndex(0), LinkIndex(3)}));
+  EXPECT_EQ(graph.composite_links(CompositeLinkIndex(1)),
+            (std::vector<LinkIndex>{LinkIndex(1), LinkIndex(2)}));
+
+  EXPECT_EQ(ssize(forest.composite_mobods()), 2);
+  EXPECT_EQ(forest.composite_mobods(CompositeMobodIndex(0)),
+            (std::vector<MobodIndex>{MobodIndex(0), MobodIndex(3)}));
+  EXPECT_EQ(forest.composite_mobods(CompositeMobodIndex(1)),
+            (std::vector<MobodIndex>{MobodIndex(1), MobodIndex(2)}));
+
+  // Remodel making single Mobods for composite links.
+  graph.BuildForest(ModelingOptions::CombineCompositeLinks);
+  forest.SanityCheckForest();
+  forest.DumpForest("WorldCompositeComesFirst -- combining");
+  EXPECT_EQ(ssize(forest.mobods()), 3);  // Because we're combining.
+  EXPECT_EQ(forest.mobods(MobodIndex(0)).follower_links(),
+            (std::vector<LinkIndex>{LinkIndex(0), LinkIndex(3)}));
+  EXPECT_EQ(forest.mobods(MobodIndex(1)).follower_links(),
+            (std::vector<LinkIndex>{LinkIndex(1), LinkIndex(2)}));
+  EXPECT_EQ(forest.mobods(MobodIndex(2)).follower_links(),
+            (std::vector<LinkIndex>{LinkIndex(4)}));
+
+  EXPECT_EQ(ssize(graph.composite_links()), 2);  // no change expected
+  EXPECT_EQ(ssize(forest.composite_mobods()), 1);  // just World now
+}
+
 }  // namespace internal
 }  // namespace multibody
 }  // namespace drake