Fill out SpanningForest structure and describe algorithm. (#21120)

Co-Authored-By: Jeremy Nimmer <[email protected]>

multibody/topology/BUILD.bazel

@@ -37,6 +37,7 @@ drake_cc_library(
     srcs = [
         "link_joint_graph.cc",
         "spanning_forest.cc",
+        "spanning_forest_mobod.cc",
     ],
     hdrs = [
         "forest.h",
@@ -48,9 +49,14 @@ drake_cc_library(
         "link_joint_graph_link.h",
         "link_joint_graph_loop_constraint.h",
         "spanning_forest.h",
+        "spanning_forest_inlines.h",
+        "spanning_forest_loop_constraint.h",
+        "spanning_forest_mobod.h",
+        "spanning_forest_tree.h",
     ],
     deps = [
         "//common:copyable_unique_ptr",
+        "//common:unused",
         "//multibody/tree:multibody_tree_indexes",
     ],
 )

multibody/topology/forest.h

@@ -12,7 +12,10 @@ reasonable. */
 // Don't alpha-sort these internal includes; the order matters.
 // clang-format off
 #include "drake/multibody/topology/spanning_forest.h"
-// TODO(sherm1) More to come.
+#include "drake/multibody/topology/spanning_forest_mobod.h"
+#include "drake/multibody/topology/spanning_forest_tree.h"
+#include "drake/multibody/topology/spanning_forest_loop_constraint.h"
+#include "drake/multibody/topology/spanning_forest_inlines.h"
 // clang-format on
 
 #undef DRAKE_MULTIBODY_TOPOLOGY_FOREST_INCLUDED

multibody/topology/link_joint_graph.h

@@ -350,18 +350,18 @@ class LinkJointGraph {
   MobodIndex link_to_mobod(BodyIndex index) const;  // See below
 
   /** After the SpanningForest has been built, returns groups of Links that are
-  welded together, which we call "Link Composites". Each group may be modeled
+  welded together, which we call "LinkComposites". 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 Link Composite is the "active link", the one whose
-  (non-weld) Joint moves the whole Link Composite due to its modeling in the
+  first entry in each LinkComposite is the "active link", the one whose
+  (non-weld) Joint moves the whole LinkComposite due to its modeling in the
   Spanning Forest. The rest of the Links in the composite are listed in no
   particular order.
 
-  The 0th Link Composite is always present and its first entry is World
+  The 0th LinkComposite is always present and its first entry is World
   (Link 0), even if nothing else is welded to World. Otherwise, composites
   are present here if they contain two or more welded Links; Links that aren't
-  welded to any other Links are not included here at all. Link Composites
+  welded to any other Links are not included here at all. LinkComposites
   are discovered as a side effect of Forest-building; there is no cost to
   accessing them here. */
   const std::vector<std::vector<BodyIndex>>& link_composites() const {
@@ -428,7 +428,7 @@ class LinkJointGraph {
                                   JointIndex primary_joint_index);
 
   // The World Link must already be in the graph but there are no Link
-  // Composites yet. This creates the 0th Link Composite and puts World in it.
+  // Composites yet. This creates the 0th LinkComposite and puts World in it.
   void CreateWorldLinkComposite();
 
   LoopConstraintIndex AddLoopClosingWeldConstraint(BodyIndex primary_link_index,

multibody/topology/link_joint_graph_joint.h

@@ -110,8 +110,6 @@ class LinkJointGraph::Joint {
       how_modeled_ = old_to_new[std::get<MobodIndex>(how_modeled_)];
   }
 
-  struct IgnoredLoopJoint {};
-
   JointIndex index_;
   std::string name_;
   ModelInstanceIndex model_instance_;
@@ -130,10 +128,7 @@ class LinkJointGraph::Joint {
   // - LinkCompositeIndex: not modeled because this is a weld interior to
   //     the indicated composite and we are combining so that one Mobod serves
   //     the whole composite.
-  // - IgnoredLoopJoint: not modeled because we intentionally ignored the Joint
-  //     (used with the IgnoreLoopJoints modeling option)
-  std::variant<std::monostate, MobodIndex, LinkCompositeIndex, IgnoredLoopJoint>
-      how_modeled_;
+  std::variant<std::monostate, MobodIndex, LinkCompositeIndex> how_modeled_;
 };
 
 }  // namespace internal

multibody/topology/spanning_forest.cc

@@ -2,7 +2,10 @@
 #include <queue>
 #include <stack>
 #include <utility>
+#include <vector>
 
+#include "drake/common/drake_assert.h"
+#include "drake/common/unused.h"
 #include "drake/multibody/topology/forest.h"
 
 namespace drake {
@@ -36,7 +39,8 @@ SpanningForest& SpanningForest::operator=(SpanningForest&& source) {
 }
 
 void SpanningForest::FixInternalPointers() {
-  // TODO(sherm1) Nothing yet, see #20225.
+  // Only Tree currently has a back pointer.
+  for (auto& tree : data_.trees) tree.forest_ = this;
 }
 
 // Clear() leaves this with nothing, not even a Mobod for World.
@@ -47,18 +51,211 @@ void SpanningForest::Clear() {
   data_.graph = saved_graph;
 }
 
-// This is the algorithm that takes an arbitrary link-joint graph and models
-// it as a spanning forest of mobilized bodies plus loop-closing constraints.
-// TODO(sherm1) Just a stub for now.
+// TODO(sherm1) The following describes the full algorithm but the code
+//  currently just has stubs. See PR #20225 for the implementation.
+
+/* This is the algorithm that takes an arbitrary link-joint graph and models
+it as a spanning forest of mobilized bodies plus loop-closing constraints.
+
+@note An assumption that affects some implementation choices: in a large system,
+most links will be either (1) welded to World or to each other to create complex
+objects, or (2) free bodies representing manipulands. The number of articulated
+links representing a robot or robots will be modest in comparison. Hence we
+want to handle welded and free links efficiently.
+
+The algorithm has three phases:
+  1 Produce the best tree structures we can. For example, when breaking
+    loops we want to minimize the maximum branch length for better numerics,
+    but we can't allow massless terminal bodies unless they are welded to
+    a massful body. Welded-together Links form a LinkComposite which can
+    (optionally) be modeled using a single mobilized body.
+  2 Reorder the forest nodes (mobilized bodies) depth first for optimal
+    computation. Each tree will consist only of consecutively-numbered nodes.
+  3 Assign joint coordinates q and velocities v sequentially according
+    to the depth-first ordering.
+
+  During execution of this algorithm, we opportunistically collect information
+  that can be used for fast queries at run time, both for the graph and the
+  forest. For example: Which Links are welded together (LinkComposites)?
+  Which Mobods are welded together (WeldedMobod groups)? Which of
+  those are anchored to World? Which Tree does a given Mobod belong to? How
+  many coordinates are inboard or outboard of a Mobod?
+
+ForestBuildingOptions (global or per-model instance) determine
+  - whether we produce a mobilized body for _each_ Link or just for each
+    _group_ of welded-together Links (a LinkComposite), and
+  - what kind of Joint we use to mobilize unconnected root Links: 0 dof fixed,
+    6 dof roll-pitch-yaw floating, or 6 dof quaternion floating.
+
+Here is an expansion of the above three phases:
+
+1. Construct a forest with good topology
+   - Add the World mobilized body to the forest at level 0.
+   - Add missing Joints to World if needed for (a) Static Links,
+     and (b) Links marked "MustBeBaseBody". (Links can be static due
+     to individual LinkFlags or membership in a static model instance.)
+   - ExtendTrees(all existing base bodies)   (see algorithm below)
+   - While there are unprocessed _jointed_ (articulated) Links
+       - Choose an unprocessed Link L and model it with a new Mobod B that is
+         to be the root node (base body) of a new tree. L is chosen by picking
+         the "best" base Link according to some policy.
+         (Heuristic: pick one that never appears as a child of a Joint.)
+       - ExtendTrees(B)  (just this one tree; see algorithm below)
+   - While there are unprocessed _unjointed_ Links (single bodies)
+       - Each of these can be the base body of a new one-Mobod tree
+       - If a Link is in a "use fixed base" model instance, weld it to World.
+         If we're combining welded links it just joins the World LinkComposite
+         and does not form a new Tree.
+       - Otherwise add a floating Joint to World (rpy or quaternion depending
+         on options) and start a new Tree.
+       - Note that any unjointed _static_ Link, or a Link in a static model
+         instance, had a weld joint added in the second step above so was
+         processed in the first call to ExtendTrees().
+
+   Algorithm ExtendTrees(tree_base_bodies):
+       - Extend each tree one level at a time.
+       - Process all level 1 (base) Links directly jointed to World.
+       - Then process all level 2 Links jointed to level 1 Links, etc.
+       - Composites are (optionally) modeled with a single Mobod (one level).
+       - If we encounter a Link that has already been processed there is a
+         loop. Split the Link onto primary and shadow Mobods and add a loop
+         constraint to weld them back together.
+
+2. Reorder depth first
+   - Determine the depth first re-ordering of the mobilized bodies.
+   - Use that reordering to update the SpanningForest in place.
+
+3. Assign coordinates q and velocities v
+   - Each tree gets a consecutive set of q's and a consecutive set of v's,
+     doled out following the depth-first ordering of the trees.
+   - Build maps from q and v to corresponding Mobod (includes fast q,v->Tree
+     also).
+*/
 void SpanningForest::BuildForest() {
   Clear();  // In case we're reusing this forest.
 
-  // Model the World (Link 0) with mobilized body 0. This also starts the
-  // 0th Link Composite in the graph.
-  // TODO(sherm1) No Mobods here yet.
+  // Model the World (Link 0) with mobilized body 0. This also starts the 0th
+  // LinkComposite in the graph and the 0th Welded Mobods group in the Forest.
+  data_.mobods.emplace_back(MobodIndex(0), BodyIndex(0));
+  data_.welded_mobods.emplace_back(std::vector{MobodIndex(0)});
+  data_.mobods[MobodIndex(0)].welded_mobods_index_ = WeldedMobodsIndex(0);
   data_.graph->set_primary_mobod_for_link(BodyIndex(0), MobodIndex(0),
                                           JointIndex{});
   data_.graph->CreateWorldLinkComposite();
+
+  data_.forest_height = 1;  // Just World so far.
+
+  // Decide on forward/reverse mobilizers; combine composite links onto a single
+  // Mobod; choose links to serve as base (root) bodies and add 6dof mobilizers
+  // for them; split loops; add shadow bodies and weld constraints.
+  ChooseForestTopology();
+
+  // Determine the desired depth-first ordering and apply it.
+  const std::vector<MobodIndex> old_to_new = CreateDepthFirstReordering();
+  FixupForestToUseNewNumbering(old_to_new);
+
+  // Dole out the q's and v's, depth-first.
+  AssignCoordinates();
+}
+
+void SpanningForest::ChooseForestTopology() {
+  // Stub; see #20225.
+
+  // Fill in World info that will be calculated in the implementation.
+  Mobod& world = data_.mobods[MobodIndex(0)];
+  world.nq_outboard_ = 0;  // Nothing other than World.
+  world.nv_outboard_ = 0;
+  // World isn't part of a tree but gets counted here.
+  world.num_subtree_mobods_ = 1;
+}
+
+/* Given the forest of mobilized bodies numbered in some arbitrary
+order, reorder them depth-first. (The arbitrary numbering was produced by the
+construction algorithm which may, for example, prefer to balance chain
+lengths when breaking loops.) Considering the result as a forest of trees
+rooted at their base bodies, the new numbering has the property that all mobods
+in tree i have lower indexes than any mobod in tree i+1. And within a tree,
+all mobods in the leftmost branch have lower numbers than any mobod in the
+next branch. */
+std::vector<MobodIndex> SpanningForest::CreateDepthFirstReordering() const {
+  DRAKE_DEMAND(ssize(mobods()) == 1);  // Just World in this stub.
+  const std::vector<MobodIndex> old_to_new{MobodIndex(0)};  // 0 maps to 0.
+  // Stub; see #20225.
+  return old_to_new;
+}
+
+/* Applies the given mapping to renumber all Mobods and references to them
+within the forest and its owning graph. */
+void SpanningForest::FixupForestToUseNewNumbering(
+    const std::vector<MobodIndex>& old_to_new) {
+  // Stub; see #20225.
+  unused(old_to_new);
+}
+
+/* Dole out the q's and v's to each of the Mobods, following the depth-first
+ordering. */
+void SpanningForest::AssignCoordinates() {
+  // Stub; see #20225.
+}
+
+// TODO(sherm1) Remove this.
+// To permit testing the APIs of Tree and LoopConstraint before the implementing
+// code is merged, we'll stub a forest that looks like this:
+//
+//            -> mobod1 => mobod2
+//     World                 ^
+//            -> mobod3 .....|  loop constraint
+//
+// There are two trees and a loop constraint where mobod3 is primary and
+// mobod2 is the shadow. The two joints to World have 1 dof, the "=>" joint
+// is a weld with 0 dofs.
+//
+// Note that there are no graph elements corresponding to any of this stuff
+// in the stub; we're just testing SpanningForest APIs here which don't care.
+// This will not be a well-formed forest for other purposes!
+void SpanningForest::AddStubTreeAndLoopConstraint() {
+  // Add three dummy Mobods.
+  data_.mobods.reserve(4);  // Prevent invalidation of the references.
+  auto& mobod1 =
+      data_.mobods.emplace_back(MobodIndex(1), BodyIndex(1), JointIndex(1),
+                                1 /* level */, false /* is_reversed */);
+  auto& mobod2 =
+      data_.mobods.emplace_back(MobodIndex(2), BodyIndex(2), JointIndex(2),
+                                2 /* level */, false /* is_reversed */);
+  auto& mobod3 =
+      data_.mobods.emplace_back(MobodIndex(3), BodyIndex(3), JointIndex(3),
+                                1 /* level */, false /* is_reversed */);
+
+  // Assign depth-first coordinates.
+  mobod1.q_start_ = 0;
+  mobod1.nq_ = 1;
+  mobod1.v_start_ = 0;
+  mobod1.nv_ = 1;
+  mobod2.q_start_ = 1;
+  mobod2.nq_ = 0;
+  mobod2.v_start_ = 1;
+  mobod2.nv_ = 0;
+  mobod3.q_start_ = 1;
+  mobod3.nq_ = 1;
+  mobod3.v_start_ = 1;
+  mobod3.nv_ = 1;
+
+  // Make the trees.
+  data_.trees.reserve(2);
+  auto& tree0 = data_.trees.emplace_back(this, TreeIndex(0), MobodIndex(1));
+  tree0.last_mobod_ = MobodIndex(2);
+  tree0.height_ = 2;
+  auto& tree1 = data_.trees.emplace_back(this, TreeIndex(1), MobodIndex(3));
+  tree1.last_mobod_ = MobodIndex(3);
+  tree1.height_ = 1;
+
+  mobod1.tree_index_ = tree0.index();
+  mobod2.tree_index_ = tree0.index();
+  mobod3.tree_index_ = tree1.index();
+
+  // Add the loop constraint.
+  data_.loop_constraints.emplace_back(LoopConstraintIndex(0), MobodIndex(3),
+                                      MobodIndex(2));
 }
 
 SpanningForest::Data::Data() = default;