[multibody] Prevent multibody/topology from splitting World to break a loop.

multibody/topology/link_joint_graph.cc

@@ -661,6 +661,7 @@ LinkOrdinal LinkJointGraph::AddShadowLink(LinkOrdinal primary_link_ordinal,
                                           bool shadow_is_parent) {
   /* Caution: this Link reference will be invalid after the emplace. */
   const Link& primary_link = links(primary_link_ordinal);
+  DRAKE_DEMAND(!primary_link.is_world());  // We never split World.
   const LinkIndex primary_link_index = primary_link.index();
   const int shadow_num = primary_link.num_shadows() + 1;
   /* Name should be <primary_name>$<shadow_num> (unique within primary's model

multibody/topology/link_joint_graph.h

@@ -84,7 +84,9 @@ correspondence between elements of the graph and the forest:
   - A user-specified Link may get split into a "primary" Link and one or more
     "shadow" Links in order to break loops. Each of those follows its own Mobod,
     so a user Link can generate multiple Mobods. (Geometry should remain
-    attached to the Mobod followed by the primary Link.)
+    attached to the Mobod followed by the primary Link.) World is the one Link
+    that is never split; a loop that closes on World is broken by splitting the
+    Link at the other end of the loop-closing Joint instead.
   - A primary Link and its shadows must be welded together by weld constraints
     which will be included in the forest and added as an ephemeral element to
     the graph.
@@ -830,6 +832,7 @@ class LinkJointGraph {
   // connects a "parent" link to a "child" link; that ordering determines the
   // sign convention for its multipliers (forces). We always make the Primary
   // the weld's parent link, and the Shadow its child.
+  // @pre the primary link is not World (we never split World).
   LinkOrdinal AddShadowLink(LinkOrdinal primary_link_ordinal,
                             JointOrdinal shadow_joint_ordinal,
                             bool shadow_is_parent);

multibody/topology/spanning_forest.cc

@@ -123,9 +123,10 @@ numbering here for clarification.
       E.4 WeldedLinksAssemblies are (optionally) modeled using fused Mobods
           where permitted (one level per Mobod).
       E.5 If we encounter a Link that has already been processed there is a
-          loop. Split the Link into primary and shadow Links, and allocate
-          two Mobods, one for each of those to follow. Add a loop constraint to
-          weld them back together.
+          loop. Split one of the loop-closing Joint's two Links (never World)
+          into primary and shadow Links, and allocate two Mobods, one for each
+          of those to follow. Add a loop constraint to weld them back together.
+          See HandleLoopClosure() for the choice of which Link to split.
       E.6 Don't end a level with a massless Link; if we encounter one
           keep going until we can end on a massful Link.
 
@@ -863,43 +864,80 @@ void SpanningForest::HandleLoopClosure(JointOrdinal loop_joint_ordinal) {
   DRAKE_DEMAND(link_is_already_in_forest(parent_ordinal) &&
                link_is_already_in_forest(child_ordinal));
 
-  /* If one of the two bodies is massless, that's no problem - we just have
-  to be sure to cut the massful one. The two branches will then each be
-  terminated with massful bodies (at 1/2 the mass each). However, if both
-  bodies are massless this forest can only be used for kinematics. */
-  const bool parent_is_massless =
-      graph().link_and_its_assembly_are_massless(parent_ordinal);
-  const bool child_is_massless =
-      graph().link_and_its_assembly_are_massless(child_ordinal);
-
-  /* Save an explanation the first time we are forced to end a branch with
-  a massless body. */
-  if (parent_is_massless && child_is_massless && data_.dynamics_ok) {
-    data_.dynamics_ok = false;
-    const Link& parent_link = links(parent_ordinal);
-    const Link& child_link = links(child_ordinal);
-    data_.why_no_dynamics = fmt::format(
-        "Loop breaks at joint {} between two massless links {} and {}. "
-        "That means these links are terminal bodies in the tree which "
-        "will produce a singular mass matrix. Hence this model cannot "
-        "be used for dynamics.\n",
-        loop_joint.name(), parent_link.name(), child_link.name());
-  }
-
-  /* If the branches leading to each link are of unequal length, we prefer to
-  split the one on the longer branch to keep the branches more even. Otherwise
-  we prefer to split the child since that will preserve the joint's
-  parent->child order in the inboard->outboard order for the Mobod. */
+  /* We are never willing to split World. A Joint can't connect a Link to itself
+  so at most one of these Links can be World. */
+  const bool parent_is_world = links(parent_ordinal).is_world();
+  const bool child_is_world = links(child_ordinal).is_world();
+  DRAKE_DEMAND(!(parent_is_world && child_is_world));
+
+  /* Whichever Link we split, its shadow gets a share of that _individual_
+  Link's mass properties, so Link::is_massless() (rather than the
+  assembly-aware LinkJointGraph::link_and_its_assembly_are_massless()) is what
+  determines whether the new shadow body will be massless. A Link that is
+  massless but welded into a massful assembly is effectively massful where it
+  sits, but a shadow of it split off onto its own Mobod is not.
+
+  If one of the two Links is massless, that's no problem - we just have to be
+  sure to cut the massful one. The two branches will then each be terminated
+  with massful bodies (at 1/2 the mass each). However, if both Links are
+  massless this forest can only be used for kinematics. The same is true if the
+  only massful one of the two is World, since we have to cut the massless one in
+  that case. */
+  const bool parent_shadow_would_be_massless =
+      links(parent_ordinal).is_massless();
+  const bool child_shadow_would_be_massless =
+      links(child_ordinal).is_massless();
+
+  /* Decide which of the two Links to split. If one of them is World we have no
+  choice - we must split the other one. Otherwise, if just one of them would
+  produce a massless shadow we must split the other one. If neither would and
+  the branches leading to each link are of unequal length, we prefer to split
+  the one on the longer branch to keep the branches more even. Otherwise we
+  prefer to split the child since that will preserve the joint's parent->child
+  order in the inboard->outboard order for the Mobod. */
 
   bool split_parent = false;  // Prefer child.
-  if (!(child_is_massless || parent_is_massless)) {
+  if (parent_is_world || child_is_world) {
+    split_parent = child_is_world;  // Split whichever one isn't World.
+  } else if (!(child_shadow_would_be_massless ||
+               parent_shadow_would_be_massless)) {
     const int child_level =
         mobods(graph().link_to_mobod(loop_joint.child_link_index())).level();
     const int parent_level =
         mobods(graph().link_to_mobod(loop_joint.parent_link_index())).level();
     if (parent_level > child_level) split_parent = true;
-  } else if (child_is_massless) {
-    split_parent = true;
+  } else if (child_shadow_would_be_massless) {
+    split_parent = true;  // Split the massful one.
+  }
+
+  /* Save an explanation the first time we are forced to split a massless Link.
+  The branch ending at the shadow of the split Link then terminates with a
+  massless body (getting a share of no mass at all). */
+  const bool split_link_is_massless = split_parent
+                                          ? parent_shadow_would_be_massless
+                                          : child_shadow_would_be_massless;
+  if (split_link_is_massless && data_.dynamics_ok) {
+    data_.dynamics_ok = false;
+    const Link& parent_link = links(parent_ordinal);
+    const Link& child_link = links(child_ordinal);
+    const Link& split_link = split_parent ? parent_link : child_link;
+    if (parent_is_world || child_is_world) {
+      data_.why_no_dynamics = fmt::format(
+          "Loop breaks between World and massless link {}, with joint {} "
+          "retargeted to the shadow link. Since World can't be split we had to "
+          "split {} instead, so its shadow is a terminal, massless body in "
+          "the tree which will produce a singular mass matrix. Hence this "
+          "model cannot be used for dynamics.\n",
+          split_link.name(), loop_joint.name(), split_link.name());
+    } else {
+      data_.why_no_dynamics = fmt::format(
+          "Loop breaks between two massless links {} and {}, with joint {} "
+          "retargeted to the shadow link. Whichever one we split, its shadow "
+          "is a terminal, massless body in the tree which will produce a "
+          "singular mass matrix. Hence this model cannot be used for "
+          "dynamics.\n",
+          parent_link.name(), child_link.name(), loop_joint.name());
+    }
   }
 
   AddShadowMobod(split_parent ? parent_ordinal : child_ordinal,
@@ -912,6 +950,7 @@ between the shadow and its primary. */
 const SpanningForest::Mobod& SpanningForest::AddShadowMobod(
     LinkOrdinal primary_link_ordinal, JointOrdinal shadow_joint_ordinal) {
   const Link& primary_link = links(primary_link_ordinal);
+  DRAKE_DEMAND(!primary_link.is_world());  // We never split World.
   Joint& shadow_joint = mutable_graph().mutable_joint(shadow_joint_ordinal);
   DRAKE_DEMAND(shadow_joint.connects(primary_link.index()));
   const LinkIndex inboard_link_index =

multibody/topology/spanning_forest.h

@@ -62,7 +62,11 @@ connect free-floating input graphs to World, and how they should be connected
 
 These are the properties the resulting forest _must_ have:
   - All nodes (Mobods) have a path from World.
-  - Massless bodies never appear as terminal nodes in the forest.
+  - World is modeled by Mobod 0 and is never cut, so World never has a shadow
+    Link.
+  - Massless bodies never appear as terminal nodes in the forest, unless that
+    is unavoidable. In that case we still build the forest but mark it unusable
+    for dynamics (see dynamics_ok() and why_no_dynamics()).
   - Mobod nodes are numbered depth-first.
   - Position and velocity coordinates q and v are assigned to the edges
     (mobilizers) with the same depth-first ordering.
@@ -92,11 +96,17 @@ fused Mobods.
 Every Link is associated with a single Mobod in the SpanningForest. Multiple
 Links (contained in a WeldedLinksAssembly) may be represented by a single Mobod.
 A mobilizer connects each Mobod to an inboard Mobod (except for World). An
-additional "shadow" Link and its Mobod is created whenever a loop is broken by
-cutting a Link, and a LoopConstraint is added to reconnect the primary Link to
-its shadow. Every moving Joint will map to a Mobod and there will be additional
-floating or weld joints added as needed to connect tree root nodes (a.k.a. "base
-bodies") to World.
+additional "shadow" Link is created whenever a loop is broken by cutting a
+Link (we call that the "primary" Link). One of the joints that connects to
+the primary link is retargeted to the shadow; we refer to that as the "loop
+joint". A loop joint is modeled identically to the original joint other than
+its frame on the primary Link has been moved to the shadow. A LoopConstraint is
+added to reconnect the primary Link to its shadow. World is never the Link we
+cut so if a loop closes on World we cut the Link at the other end of the
+loop-closing Joint, even if that Link is massless (in which case the forest
+can't be used for dynamics). Every moving Joint will map to a Mobod and there
+will be additional floating or weld joints added as needed to connect tree
+root nodes (a.k.a. "base bodies") to World.
 
 When extra bodies, mobilizers, and constraints are needed to construct the
 forest, corresponding _ephemeral_ Links, Joints, and LoopConstraints are added
@@ -575,11 +585,16 @@ class SpanningForest {
   // splitting off a shadow of one of the Links and mobilizing the shadow with a
   // forward or reversed Mobilizer of the Joint's type. Then we add a Weld
   // Constraint to attach the shadow to its primary. Some details:
-  //  - if one link is massless, split the other one
-  //  - if both are massless we have an invalid forest (can't be used for
-  //    dynamics)
-  //  - either or both Links may be part of a WeldedLinksAssembly; it is the
-  //    mass properties of the whole assembly that determines masslessness.
+  //  - we never split World, so if one of the Links is World we must split
+  //    the other one whether or not it is massless
+  //  - otherwise, if one link is massless, split the other one
+  //  - if we are forced to split a massless link (because both are massless,
+  //    or because the massful one is World) we have an invalid forest (can't
+  //    be used for dynamics)
+  //  - "massless" here means the individual Link is massless (see
+  //    Link::is_massless()) because a shadow gets a share of just its primary
+  //    Link's mass properties. In particular, a massless Link that is welded
+  //    into a massful WeldedLinksAssembly still yields a massless shadow.
   void HandleLoopClosure(JointOrdinal loop_joint_ordinal);
 
   // Adds a shadow Link of the given primary and mobilizes the shadow with

multibody/topology/spanning_forest_debug.cc

@@ -69,6 +69,7 @@ void SpanningForest::SanityCheckForest() const {
 
   const LinkJointGraph::Link& world_link = links(LinkOrdinal(0));
   DRAKE_THROW_UNLESS(world_link.mobod_index() == 0);
+  DRAKE_THROW_UNLESS(world_link.num_shadows() == 0);  // We never split World.
   DRAKE_THROW_UNLESS(world_link.welded_links_assembly().has_value());
   DRAKE_THROW_UNLESS(world_link.welded_links_assembly() ==
                      WeldedLinksAssemblyIndex(0));