Adds a sanity checker for graph & forest (#21818)

sherm1 · web-flow · commit b1b1312d79ed · 2024-08-19T16:22:50.000-07:00
diff --git a/multibody/topology/spanning_forest.cc b/multibody/topology/spanning_forest.cc
@@ -167,6 +167,8 @@ bool SpanningForest::BuildForest() {
   /* Dole out the q's and v's, depth-first. (Phase 3) */
   AssignCoordinates();
 
+  DRAKE_ASSERT_VOID(SanityCheckForest());
+
   return dynamics_ok();
 }
 
diff --git a/multibody/topology/spanning_forest.h b/multibody/topology/spanning_forest.h
@@ -405,6 +405,11 @@ class SpanningForest {
   @see LinkJointGraph::GenerateGraphvizString() */
   std::string GenerateGraphvizString(std::string_view label) const;
 
+  /** (Debugging, Testing) Runs a series of expensive tests to see that the
+  Graph and Forest are internally consistent and throws if not. Does nothing
+  if no Forest has been built. */
+  void SanityCheckForest() const;
+
  private:
   friend class LinkJointGraph;
   friend class copyable_unique_ptr<SpanningForest>;
diff --git a/multibody/topology/spanning_forest_debug.cc b/multibody/topology/spanning_forest_debug.cc
@@ -4,6 +4,140 @@ namespace drake {
 namespace multibody {
 namespace internal {
 
+void SpanningForest::SanityCheckForest() const {
+  // Should always have a LinkJointGraph backpointer even if empty.
+  DRAKE_THROW_UNLESS(data_.graph != nullptr);
+  if (mobods().empty()) return;
+
+  const Mobod& world_mobod = mobods(MobodIndex(0));
+  DRAKE_THROW_UNLESS(world_mobod.is_world());
+  DRAKE_THROW_UNLESS(!world_mobod.is_base_body());
+  DRAKE_THROW_UNLESS(world_mobod.is_anchored());
+  DRAKE_THROW_UNLESS(world_mobod.has_massful_follower_link());
+  DRAKE_THROW_UNLESS(world_mobod.index() == MobodIndex(0));
+  DRAKE_THROW_UNLESS(world_mobod.link_ordinal() == LinkOrdinal(0));
+  DRAKE_THROW_UNLESS(!world_mobod.joint_ordinal().is_valid());
+  DRAKE_THROW_UNLESS(!world_mobod.tree().is_valid());
+  DRAKE_THROW_UNLESS(world_mobod.level() == 0);
+  DRAKE_THROW_UNLESS(!world_mobod.inboard().is_valid());
+  DRAKE_THROW_UNLESS(ssize(world_mobod.outboards()) == ssize(trees()));
+
+  /* WeldedMobods groups and LinkComposites are different but in either case
+  there is always a World group or composite, it must come first, and the World
+  Link or Mobod must be listed first in that group or composite. */
+  DRAKE_THROW_UNLESS(world_mobod.welded_mobods_group().has_value());
+  DRAKE_THROW_UNLESS(world_mobod.welded_mobods_group() == WeldedMobodsIndex(0));
+  DRAKE_THROW_UNLESS(welded_mobods()[0][0] == MobodIndex(0));
+
+  const LinkJointGraph::Link& world_link = links(LinkOrdinal(0));
+  DRAKE_THROW_UNLESS(world_link.mobod_index() == 0);
+  DRAKE_THROW_UNLESS(world_link.composite().has_value());
+  DRAKE_THROW_UNLESS(world_link.composite() == LinkCompositeIndex(0));
+  DRAKE_THROW_UNLESS(graph().link_composites()[0].links[0] == LinkIndex(0));
+
+  for (MobodIndex index(1); index < ssize(mobods()); ++index) {
+    const Mobod& mobod = mobods(index);
+    DRAKE_THROW_UNLESS(mobod.index() == index);
+    DRAKE_THROW_UNLESS(links(mobod.link_ordinal()).mobod_index() == index);
+    DRAKE_THROW_UNLESS(joints(mobod.joint_ordinal()).mobod_index() == index);
+
+    // The mobod's Tree must include the mobod.
+    const Tree& tree = trees(mobod.tree());
+    DRAKE_THROW_UNLESS(tree.base_mobod() <= mobod.index() &&
+                       mobod.index() <= tree.last_mobod());
+
+    // If the mobod is part of a WeldedMobods group, that group must include it!
+    if (mobod.welded_mobods_group().has_value()) {
+      const auto& welded_group = welded_mobods()[*mobod.welded_mobods_group()];
+      DRAKE_THROW_UNLESS(std::find(welded_group.begin(), welded_group.end(),
+                                   index) != welded_group.end());
+    }
+  }
+
+  /* Check that Links alleged to be in a LinkComposite agree that they are,
+  and that massless LinkComposites are composed only of massless Links. */
+  for (LinkCompositeIndex composite_index(0);
+       composite_index < ssize(graph().link_composites()); ++composite_index) {
+    const LinkJointGraph::LinkComposite& composite =
+        graph().link_composites(composite_index);
+    bool all_links_are_massless = true;
+    for (const LinkIndex& link_index : composite.links) {
+      const LinkJointGraph::Link& link = link_by_index(link_index);
+      DRAKE_THROW_UNLESS(link.composite() == composite_index);
+      if (!link.is_massless()) all_links_are_massless = false;
+    }
+    DRAKE_THROW_UNLESS(composite.is_massless == all_links_are_massless);
+  }
+
+  /* Check that all the Links and Joints have Mobods that point back to them. */
+  for (LinkOrdinal link_ordinal{0}; link_ordinal < ssize(links());
+       ++link_ordinal) {
+    const Link& link = links(link_ordinal);
+    DRAKE_THROW_UNLESS(link.ordinal() == link_ordinal);
+    DRAKE_THROW_UNLESS(&link_by_index(link.index()) == &link);
+    DRAKE_THROW_UNLESS(mobods(link.mobod_index()).HasFollower(link_ordinal));
+  }
+  for (JointOrdinal joint_ordinal{0}; joint_ordinal < ssize(joints());
+       ++joint_ordinal) {
+    const Joint& joint = joints(joint_ordinal);
+    DRAKE_THROW_UNLESS(joint.ordinal() == joint_ordinal);
+    DRAKE_THROW_UNLESS(&joint_by_index(joint.index()) == &joint);
+    if (!joint.mobod_index().is_valid()) continue;  // Not modeled
+    DRAKE_THROW_UNLESS(mobods(joint.mobod_index()).joint_ordinal() ==
+                       joint_ordinal);
+  }
+
+  /* Check tree height and make sure the tree's mobods agree they are part
+  of the tree. Make sure the forest height is the max tree height. */
+  int forest_height = 1;  // World alone has a height of 1.
+  for (const auto& tree : trees()) {
+    DRAKE_THROW_UNLESS(tree.forest_ == this);  // Check backpointer.
+    DRAKE_THROW_UNLESS(mobods(tree.base_mobod()).level() == 1);
+    DRAKE_THROW_UNLESS(tree.last_mobod() >= tree.base_mobod());
+    int computed_height = 0;
+    for (auto& mobod : tree) {
+      DRAKE_THROW_UNLESS(mobod.tree() == tree.index());
+      computed_height = std::max(computed_height, mobod.level());
+    }
+    DRAKE_THROW_UNLESS(tree.height() == computed_height);
+    forest_height = std::max(forest_height, tree.height() + 1);
+  }
+  DRAKE_THROW_UNLESS(height() == forest_height);
+
+  /* Check each WeldedMobods group to make sure:
+     - the mobods it contains agree that they are contained in that group
+     - mobods welded to World know they are "anchored"
+     - except for the World group, the first entry in each group is a
+       Mobod that has a non-weld inboard joint, and all the others are Welds.
+     - the first entry in each WeldedMobods group should map to the Link (the
+       "active link") that is listed first in its LinkComposite. */
+  for (WeldedMobodsIndex welded_mobods_index(0);
+       welded_mobods_index < ssize(welded_mobods()); ++welded_mobods_index) {
+    const std::vector<MobodIndex> welded_mobods_group =
+        welded_mobods()[welded_mobods_index];
+    for (MobodIndex mobod_index : welded_mobods_group) {
+      const Mobod& mobod = mobods(mobod_index);
+      const bool is_active_mobod = (mobod_index == welded_mobods_group[0]);
+      const bool should_be_weld =  // World or an interior body.
+          welded_mobods_index == 0 || !is_active_mobod;
+      DRAKE_THROW_UNLESS(mobod.is_weld() == should_be_weld);
+      DRAKE_THROW_UNLESS(mobod.welded_mobods_group() == welded_mobods_index);
+      const bool should_be_anchored = (welded_mobods_index == 0);
+      DRAKE_THROW_UNLESS(mobod.is_anchored() == should_be_anchored);
+
+      if (is_active_mobod) {
+        const LinkOrdinal active_link_ordinal = mobod.link_ordinal();
+        const Link& active_link = links(active_link_ordinal);
+        const std::optional<LinkCompositeIndex> link_composite =
+            active_link.composite();
+        DRAKE_THROW_UNLESS(link_composite.has_value());
+        DRAKE_THROW_UNLESS(graph().link_composites(*link_composite).links[0] ==
+                           active_link.index());
+      }
+    }
+  }
+}
+
 // TODO(sherm1) Consider accommodation for colorblind people. Avoid red/green
 //  and/or use line styles. Update the legend.
 
diff --git a/multibody/topology/test/link_joint_graph_test.cc b/multibody/topology/test/link_joint_graph_test.cc
@@ -353,9 +353,9 @@ that the wrapping worked correctly.
 
 Here is the graph:
 
-                  {9}
-                   ^
-                   |
+                  {9}                        {l} Link l
+                   ^                          -> revolute joint
+                   |                          => weld joint
            {1} -> {2} => {3} => {8}
    World
      {0}   {4} => {5} -> {6}
diff --git a/multibody/topology/test/spanning_forest_test.cc b/multibody/topology/test/spanning_forest_test.cc
@@ -57,6 +57,7 @@ GTEST_TEST(SpanningForest, WorldOnlyTest) {
   EXPECT_TRUE(graph.BuildForest());
   EXPECT_TRUE(graph.forest_is_valid());
   EXPECT_TRUE(forest.is_valid());
+  EXPECT_NO_THROW(forest.SanityCheckForest());
   EXPECT_EQ(&forest.graph(), &graph);
   const SpanningForest::Mobod& world = forest.mobods(MobodIndex(0));
   EXPECT_EQ(&world, &forest.world_mobod());
@@ -293,6 +294,7 @@ GTEST_TEST(SpanningForest, MultipleBranchesDefaultOptions) {
 
   // Build with default options.
   EXPECT_TRUE(graph.BuildForest());
+  EXPECT_NO_THROW(forest.SanityCheckForest());
 
   EXPECT_EQ(forest.options(), ForestBuildingOptions::kDefault);
   EXPECT_EQ(forest.options(left_instance), ForestBuildingOptions::kDefault);
@@ -678,6 +680,7 @@ GTEST_TEST(SpanningForest, SerialChainAndMore) {
                                  ForestBuildingOptions::kStatic);
   EXPECT_TRUE(graph.BuildForest());
   const SpanningForest& forest = graph.forest();
+  EXPECT_NO_THROW(forest.SanityCheckForest());
   EXPECT_TRUE(graph.forest_is_valid());
 
   // Verify that ChangeJointType() rejects an attempt to change a static link's
@@ -817,6 +820,7 @@ GTEST_TEST(SpanningForest, SerialChainAndMore) {
                                  ForestBuildingOptions::kMergeLinkComposites |
                                      ForestBuildingOptions::kStatic);
   EXPECT_TRUE(graph.BuildForest());
+  EXPECT_NO_THROW(forest.SanityCheckForest());
 
   // The graph shouldn't change from SpanningForest 1, but the forest will.
   EXPECT_EQ(ssize(graph.joints()) - graph.num_user_joints(), 4);
@@ -861,6 +865,7 @@ GTEST_TEST(SpanningForest, SerialChainAndMore) {
   graph.ChangeJointFlags(joint_10_11_index, JointFlags::kMustBeModeled);
   // Built the forest with same options as used for 2a.
   EXPECT_TRUE(graph.BuildForest());
+  EXPECT_NO_THROW(forest.SanityCheckForest());
 
   EXPECT_EQ(ssize(forest.mobods()), 9);
   EXPECT_EQ(ssize(forest.trees()), 3);
@@ -895,6 +900,7 @@ GTEST_TEST(SpanningForest, SerialChainAndMore) {
                                  ForestBuildingOptions::kMergeLinkComposites |
                                      ForestBuildingOptions::kStatic);
   EXPECT_TRUE(graph.BuildForest());
+  EXPECT_NO_THROW(forest.SanityCheckForest());
 
   EXPECT_EQ(ssize(forest.mobods()), 6);
   EXPECT_EQ(ssize(forest.trees()), 1);
@@ -1056,6 +1062,7 @@ GTEST_TEST(SpanningForest, WeldedSubgraphs) {
 
   EXPECT_TRUE(graph.BuildForest());
   const SpanningForest& forest = graph.forest();
+  EXPECT_NO_THROW(forest.SanityCheckForest());
 
   EXPECT_EQ(graph.num_user_links(), 14);  // Same as before building.
   EXPECT_EQ(graph.num_user_joints(), 14);
@@ -1223,6 +1230,7 @@ GTEST_TEST(SpanningForest, WeldedSubgraphs) {
   graph.SetGlobalForestBuildingOptions(
       ForestBuildingOptions::kMergeLinkComposites);
   EXPECT_TRUE(graph.BuildForest());
+  EXPECT_NO_THROW(forest.SanityCheckForest());
 
   EXPECT_EQ(ssize(graph.links()), 15);  // Only one added shadow.
   EXPECT_EQ(ssize(graph.link_composites()), 3);
@@ -1376,6 +1384,7 @@ GTEST_TEST(SpanningForest, MasslessLinksChangeLoopBreaking) {
 
   EXPECT_TRUE(graph.BuildForest());  // Using default options.
   const SpanningForest& forest = graph.forest();
+  EXPECT_NO_THROW(forest.SanityCheckForest());
 
   EXPECT_EQ(ssize(graph.links()), 8);  // Added a shadow.
   EXPECT_EQ(ssize(graph.joints()), 7);
@@ -1392,6 +1401,7 @@ GTEST_TEST(SpanningForest, MasslessLinksChangeLoopBreaking) {
   // (Tests Case 2 in ExtendTreesOneLevel())
   graph.ChangeLinkFlags(LinkIndex(3), LinkFlags::kMassless);
   EXPECT_TRUE(graph.BuildForest());
+  EXPECT_NO_THROW(forest.SanityCheckForest());
 
   // Check that links not in a composite still respond correctly.
   EXPECT_TRUE(graph.link_and_its_composite_are_massless(LinkOrdinal(3)));
@@ -1411,6 +1421,7 @@ GTEST_TEST(SpanningForest, MasslessLinksChangeLoopBreaking) {
   // (Tests Case 3 in ExtendTreesOneLevel())
   graph.ChangeLinkFlags(LinkIndex(4), LinkFlags::kMassless);
   EXPECT_TRUE(graph.BuildForest());
+  EXPECT_NO_THROW(forest.SanityCheckForest());
 
   EXPECT_EQ(ssize(graph.links()), 8);
   EXPECT_EQ(ssize(graph.joints()), 7);
@@ -1478,6 +1489,7 @@ GTEST_TEST(SpanningForest, MasslessBodiesShareSplitLink) {
 
   EXPECT_TRUE(graph.BuildForest());  // Using default options.
   const SpanningForest& forest = graph.forest();
+  EXPECT_NO_THROW(forest.SanityCheckForest());
 
   EXPECT_EQ(ssize(graph.links()), 5);  // After modeling.
   EXPECT_EQ(graph.num_user_links(), 4);
@@ -1566,6 +1578,7 @@ GTEST_TEST(SpanningForest, DoubleLoop) {
 
   EXPECT_TRUE(graph.BuildForest());  // Using default options.
   const SpanningForest& forest = graph.forest();
+  EXPECT_NO_THROW(forest.SanityCheckForest());
 
   EXPECT_EQ(ssize(graph.links()), 10);  // After modeling.
   EXPECT_EQ(ssize(graph.joints()), 9);
@@ -1668,6 +1681,7 @@ GTEST_TEST(SpanningForest, WorldCompositeComesFirst) {
 
   EXPECT_TRUE(graph.BuildForest());  // Using default options.
   const SpanningForest& forest = graph.forest();
+  EXPECT_NO_THROW(forest.SanityCheckForest());
 
   EXPECT_EQ(ssize(graph.links()), 5);
   EXPECT_EQ(ssize(forest.mobods()), 5);  // Because we're not merging.
@@ -1697,7 +1711,7 @@ GTEST_TEST(SpanningForest, WorldCompositeComesFirst) {
   graph.SetGlobalForestBuildingOptions(
       ForestBuildingOptions::kMergeLinkComposites);
   EXPECT_TRUE(graph.BuildForest());
-
+  EXPECT_NO_THROW(forest.SanityCheckForest());
   EXPECT_EQ(ssize(forest.mobods()), 3);  // Because we're merging.
   EXPECT_EQ(forest.mobods(MobodIndex(0)).follower_link_ordinals(),
             (std::vector<LinkOrdinal>{LinkOrdinal(0), LinkOrdinal(3)}));
@@ -1984,6 +1998,7 @@ GTEST_TEST(SpanningForest, LoopWithComposites) {
       ForestBuildingOptions::kMergeLinkComposites);
   EXPECT_TRUE(graph.BuildForest());
   const SpanningForest& forest = graph.forest();
+  EXPECT_NO_THROW(forest.SanityCheckForest());
 
   // After modeling
   EXPECT_EQ(ssize(graph.links()), 12);            // split one, added shadow
@@ -2022,6 +2037,7 @@ GTEST_TEST(SpanningForest, LoopWithComposites) {
   EXPECT_EQ(ssize(graph_copy.links()), 12);
   EXPECT_TRUE(graph_copy.forest_is_valid());
   const SpanningForest& copy_model = graph_copy.forest();
+  EXPECT_NO_THROW(copy_model.SanityCheckForest());
   EXPECT_NE(&copy_model, &forest);
   EXPECT_EQ(&copy_model.graph(), &graph_copy);  // backpointer
 
@@ -2030,30 +2046,35 @@ GTEST_TEST(SpanningForest, LoopWithComposites) {
   EXPECT_EQ(ssize(graph_assign.links()), 12);
   EXPECT_TRUE(graph_assign.forest_is_valid());
   EXPECT_NE(&graph_assign.forest(), &forest);
+  EXPECT_NO_THROW(graph_assign.forest().SanityCheckForest());
   EXPECT_EQ(&graph_assign.forest().graph(), &graph_assign);
 
   LinkJointGraph graph_move(std::move(graph));
   EXPECT_EQ(ssize(graph_move.links()), 12);
   EXPECT_EQ(ssize(graph.links()), 1);  // Just world now.
   EXPECT_EQ(&graph_move.forest(), &forest);
+  EXPECT_NO_THROW(graph_move.forest().SanityCheckForest());
   EXPECT_EQ(&graph_move.forest().graph(), &graph_move);
   // graph is now default-constructed so still has a forest
   EXPECT_NE(&graph.forest(), &forest);
   EXPECT_FALSE(graph.forest_is_valid());
   EXPECT_EQ(&graph.forest().graph(), &graph);
+  EXPECT_NO_THROW(graph.forest().SanityCheckForest());  // Empty but OK.
 
   LinkJointGraph graph_move_assign;
   graph_move_assign = std::move(graph_copy);
   EXPECT_EQ(ssize(graph_move_assign.links()), 12);
   EXPECT_TRUE(graph_move_assign.forest_is_valid());
   EXPECT_EQ(&graph_move_assign.forest(), &copy_model);
+  EXPECT_NO_THROW(graph_move_assign.forest().SanityCheckForest());
   EXPECT_EQ(&graph_move_assign.forest().graph(), &graph_move_assign);
   // graph_copy is now default-constructed. Should have world and a
   // new (empty) forest.
   EXPECT_EQ(ssize(graph_copy.links()), 1);
   EXPECT_NE(&graph_copy.forest(), &copy_model);
   EXPECT_FALSE(graph_copy.forest_is_valid());
   EXPECT_EQ(&graph_copy.forest().graph(), &graph_copy);
+  EXPECT_NO_THROW(graph_copy.forest().SanityCheckForest());  // Empty but OK.
 }
 
 /* Make sure massless, merged composites are working correctly. They are
@@ -2129,6 +2150,7 @@ GTEST_TEST(SpanningForest, MasslessMergedComposites) {
   EXPECT_EQ(ssize(graph.loop_constraints()), 0);
 
   EXPECT_TRUE(graph.BuildForest());
+  EXPECT_NO_THROW(forest.SanityCheckForest());
 
   // After modeling
   EXPECT_EQ(ssize(graph.links()), 10);  // added shadow 3s {9}
@@ -2169,6 +2191,7 @@ GTEST_TEST(SpanningForest, MasslessMergedComposites) {
   height of 3. */
   graph.ChangeJointType(JointIndex(6), "revolute");
   EXPECT_TRUE(graph.BuildForest());
+  EXPECT_NO_THROW(forest.SanityCheckForest());
 
   // The links are massless and so is their composite.
   for (LinkOrdinal link_ordinal(4); link_ordinal <= 6; ++link_ordinal)
@@ -2195,6 +2218,7 @@ GTEST_TEST(SpanningForest, MasslessMergedComposites) {
   graph.ChangeLinkFlags(LinkIndex(7), LinkFlags::kMassless);
   graph.ChangeLinkFlags(LinkIndex(8), LinkFlags::kMassless);
   EXPECT_TRUE(graph.BuildForest());
+  EXPECT_NO_THROW(forest.SanityCheckForest());
   const auto& newer_shadow_link = graph.link_by_index(LinkIndex(9));
   EXPECT_TRUE(newer_shadow_link.is_shadow());
   EXPECT_EQ(newer_shadow_link.name(), "link3$1");

Original file line number	Diff line number	Diff line change
`@@ -167,6 +167,8 @@ bool SpanningForest::BuildForest() {`
`167`	`167`	`/* Dole out the q's and v's, depth-first. (Phase 3) */`
`168`	`168`	`AssignCoordinates();`
`169`	`169`
	`170`	`+ DRAKE_ASSERT_VOID(SanityCheckForest());`
	`171`	`+`
`170`	`172`	`return dynamics_ok();`
`171`	`173`	`}`
`172`	`174`