From cc4de0d9d06a42a4c342295d076a367c2e17d6f1 Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Thu, 20 Aug 2026 16:45:48 -0700 Subject: [PATCH 1/3] Prevent splitting World to break a loop. --- multibody/topology/link_joint_graph.h | 5 +- multibody/topology/spanning_forest.cc | 75 ++++++++++----- multibody/topology/spanning_forest.h | 23 +++-- .../topology/test/spanning_forest_test.cc | 93 +++++++++++++++++-- 4 files changed, 158 insertions(+), 38 deletions(-) diff --git a/multibody/topology/link_joint_graph.h b/multibody/topology/link_joint_graph.h index 1d12b650d18b..af7a6c3caaff 100644 --- a/multibody/topology/link_joint_graph.h +++ b/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); diff --git a/multibody/topology/spanning_forest.cc b/multibody/topology/spanning_forest.cc index f73ee24af749..097ac206066a 100644 --- a/multibody/topology/spanning_forest.cc +++ b/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,36 +864,35 @@ void SpanningForest::HandleLoopClosure(JointOrdinal loop_joint_ordinal) { DRAKE_DEMAND(link_is_already_in_forest(parent_ordinal) && link_is_already_in_forest(child_ordinal)); + /* 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)); + /* 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. */ + bodies 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_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. */ + /* 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 is + massless we must split the massful one. If neither is massless 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_is_massless || parent_is_massless)) { const int child_level = mobods(graph().link_to_mobod(loop_joint.child_link_index())).level(); const int parent_level = @@ -902,6 +902,34 @@ void SpanningForest::HandleLoopClosure(JointOrdinal loop_joint_ordinal) { split_parent = true; } + /* Save an explanation the first time we are forced to split a massless Link. + Both branches ending at the split Link then terminate with a massless body + (each getting half of no mass at all). */ + const bool split_link_is_massless = + split_parent ? parent_is_massless : child_is_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 at joint {} between World and massless link {}. Since " + "World can't be split we had to split {} instead, so it and its " + "shadow 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(), split_link.name(), split_link.name()); + } else { + 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()); + } + } + AddShadowMobod(split_parent ? parent_ordinal : child_ordinal, loop_joint_ordinal); } @@ -912,6 +940,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 = diff --git a/multibody/topology/spanning_forest.h b/multibody/topology/spanning_forest.h index 8d7ac215d09c..5e050b505cff 100644 --- a/multibody/topology/spanning_forest.h +++ b/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. @@ -94,9 +98,11 @@ 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. +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,9 +581,12 @@ 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) + // - 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) // - either or both Links may be part of a WeldedLinksAssembly; it is the // mass properties of the whole assembly that determines masslessness. void HandleLoopClosure(JointOrdinal loop_joint_ordinal); diff --git a/multibody/topology/test/spanning_forest_test.cc b/multibody/topology/test/spanning_forest_test.cc index 17804b94a5cc..36ce834c5fe6 100644 --- a/multibody/topology/test/spanning_forest_test.cc +++ b/multibody/topology/test/spanning_forest_test.cc @@ -2005,9 +2005,9 @@ get back to {4} and realize we're forced to connect massless links {4} and {3}. But if we change link {4} to massful, that same procedure should rescue dynamics since we can end both branches with half of link {4}. -We'll also try replacing massful link 4 with World and verify that still -works the same way. We can split off an arbitrary-mass chunk of World to -terminate the massless branch. */ +We'll also try replacing massful link 4 with World. In that case dynamics is +_not_ rescued: World is never split, so we are forced to split massless link +{3} and end both branches with a massless body. */ GTEST_TEST(SpanningForest, MasslessLoopAreDetected) { LinkJointGraph graph; graph.RegisterJointType("revolute", 1, 1); @@ -2054,15 +2054,94 @@ GTEST_TEST(SpanningForest, MasslessLoopAreDetected) { LinkIndex(world_graph_joints[i].second)); } - /* Check that we split World as expected. */ - EXPECT_TRUE(world_graph.BuildForest()); + /* The loop-closing joint here is joint1, connecting World to massless + link3. Check that we split link3 rather than World, and that we report that + this model can't be used for dynamics. */ + EXPECT_FALSE(world_graph.BuildForest()); + EXPECT_THAT(world_graph.forest().why_no_dynamics(), + testing::MatchesRegex("Loop breaks.*joint1.*between World and " + "massless link link3.*World can't be " + "split.*cannot be used for dynamics.*")); EXPECT_EQ(world_graph.num_user_links(), 4); EXPECT_EQ(ssize(world_graph.links()), 5); EXPECT_EQ(ssize(world_graph.forest().mobods()), 5); - EXPECT_EQ(world_graph.world_link().num_shadows(), 1); + EXPECT_EQ(world_graph.world_link().num_shadows(), 0); EXPECT_TRUE(world_graph.link_by_index(LinkIndex(4)).is_shadow()); EXPECT_EQ(world_graph.link_by_index(LinkIndex(4)).primary_link(), - LinkIndex(0)); + LinkIndex(3)); + EXPECT_EQ(world_graph.link_by_index(LinkIndex(3)).num_shadows(), 1); +} + +/* World must not be split even when nothing is massless. A non-weld loop +joint that closes onto World from a Link that has been fused into the World +WeldedLinksAssembly used to produce a level-0 "tie" in the branch-length +heuristic, which broke the tie in favor of splitting the child -- World. Now +we should always split the other Link. + + {0}==={1}==={2} (welds, all fused onto the World Mobod) + ^ | + +--revolute---+ (a loop joint, closing back onto World) + +There should be just two Mobods: the World Mobod (followed by links {0}, {1}, +and {2}) plus a Mobod for the shadow of link {2}, mobilized by the revolute +joint and welded back to {2} by a loop constraint. Since all these links are +massful, dynamics should be fine (though not very interesting!). We try this +with the revolute joint declared in both directions to be sure the outcome +doesn't depend on which end of the loop joint the user called the parent. */ +GTEST_TEST(SpanningForest, LoopClosingOnWorldDoesNotSplitWorld) { + for (bool world_is_parent : {false, true}) { + SCOPED_TRACE(fmt::format("world_is_parent={}", world_is_parent)); + LinkJointGraph graph; + graph.RegisterJointType("revolute", 1, 1); + graph.SetGlobalForestBuildingOptions( + ForestBuildingOptions::kFuseWeldedLinksAssemblies); + graph.AddLink("link1", default_model_instance()); + graph.AddLink("link2", default_model_instance()); + graph.AddJoint("weld0", default_model_instance(), "weld", LinkIndex(0), + LinkIndex(1)); + graph.AddJoint("weld1", default_model_instance(), "weld", LinkIndex(1), + LinkIndex(2)); + graph.AddJoint("revolute", default_model_instance(), "revolute", + world_is_parent ? LinkIndex(0) : LinkIndex(2), + world_is_parent ? LinkIndex(2) : LinkIndex(0)); + + EXPECT_TRUE(graph.BuildForest()); // Dynamics is OK. + EXPECT_EQ(graph.num_user_links(), 3); + EXPECT_EQ(ssize(graph.links()), 4); // One shadow link was added. + EXPECT_EQ(graph.world_link().num_shadows(), 0); + + /* The shadow is of link2, not World. */ + const LinkJointGraph::Link& shadow = graph.link_by_index(LinkIndex(3)); + EXPECT_TRUE(shadow.is_shadow()); + EXPECT_EQ(shadow.name(), "link2$1"); + EXPECT_EQ(shadow.primary_link(), LinkIndex(2)); + EXPECT_EQ(graph.link_by_index(LinkIndex(2)).num_shadows(), 1); + + /* World, link1, and link2 all follow the World Mobod; the shadow gets its + own Mobod mobilized by the revolute joint. */ + const SpanningForest& forest = graph.forest(); + EXPECT_EQ(ssize(forest.mobods()), 2); + EXPECT_EQ(graph.link_to_mobod(LinkIndex(0)), MobodIndex(0)); + EXPECT_EQ(graph.link_to_mobod(LinkIndex(1)), MobodIndex(0)); + EXPECT_EQ(graph.link_to_mobod(LinkIndex(2)), MobodIndex(0)); + EXPECT_EQ(graph.link_to_mobod(LinkIndex(3)), MobodIndex(1)); + EXPECT_EQ(forest.mobods(MobodIndex(1)).inboard_mobod(), MobodIndex(0)); + EXPECT_EQ(graph.joints(forest.mobods(MobodIndex(1)).active_joint_ordinal()) + .name(), + "revolute"); + + /* The loop constraint welds the shadow back to its primary. */ + EXPECT_EQ(ssize(forest.loop_constraints()), 1); + const SpanningForest::LoopConstraint& loop_constraint = + forest.loop_constraints(LoopConstraintIndex(0)); + EXPECT_EQ(loop_constraint.primary_mobod(), MobodIndex(0)); + EXPECT_EQ(loop_constraint.shadow_mobod(), MobodIndex(1)); + EXPECT_EQ(ssize(graph.loop_constraints()), 1); + EXPECT_EQ(graph.loop_constraints(LoopConstraintIndex(0)).primary_link(), + LinkIndex(2)); + EXPECT_EQ(graph.loop_constraints(LoopConstraintIndex(0)).shadow_link(), + LinkIndex(3)); + } } /* WeldedLinksAssemblies should be treated the same as single bodies while From 8d6e64dc3c26dd7ed14c650a5ce97785e087d871 Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Wed, 2 Sep 2026 13:46:44 -0700 Subject: [PATCH 2/3] Respond to GPT's review --- multibody/topology/link_joint_graph.cc | 1 + multibody/topology/spanning_forest.cc | 65 +++++++++++-------- multibody/topology/spanning_forest.h | 6 +- multibody/topology/spanning_forest_debug.cc | 1 + .../topology/test/spanning_forest_test.cc | 56 ++++++++++++++++ 5 files changed, 99 insertions(+), 30 deletions(-) diff --git a/multibody/topology/link_joint_graph.cc b/multibody/topology/link_joint_graph.cc index 1ea2f768dbdd..8d0a61a6f257 100644 --- a/multibody/topology/link_joint_graph.cc +++ b/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 $ (unique within primary's model diff --git a/multibody/topology/spanning_forest.cc b/multibody/topology/spanning_forest.cc index 097ac206066a..3b6ab8eff01f 100644 --- a/multibody/topology/spanning_forest.cc +++ b/multibody/topology/spanning_forest.cc @@ -870,43 +870,52 @@ void SpanningForest::HandleLoopClosure(JointOrdinal loop_joint_ordinal) { const bool child_is_world = links(child_ordinal).is_world(); DRAKE_DEMAND(!(parent_is_world && child_is_world)); - /* 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. 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_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); + /* 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 is - massless we must split the massful one. If neither is massless 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. */ + 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 (parent_is_world || child_is_world) { split_parent = child_is_world; // Split whichever one isn't World. - } else if (!(child_is_massless || parent_is_massless)) { + } 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. - Both branches ending at the split Link then terminate with a massless body - (each getting half of no mass at all). */ - const bool split_link_is_massless = - split_parent ? parent_is_massless : child_is_massless; + 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); @@ -915,17 +924,17 @@ void SpanningForest::HandleLoopClosure(JointOrdinal loop_joint_ordinal) { if (parent_is_world || child_is_world) { data_.why_no_dynamics = fmt::format( "Loop breaks at joint {} between World and massless link {}. Since " - "World can't be split we had to split {} instead, so it and its " - "shadow are terminal bodies in the tree which will produce a " + "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", loop_joint.name(), split_link.name(), split_link.name()); } else { 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", + "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", loop_joint.name(), parent_link.name(), child_link.name()); } } diff --git a/multibody/topology/spanning_forest.h b/multibody/topology/spanning_forest.h index 5e050b505cff..8055fac47f55 100644 --- a/multibody/topology/spanning_forest.h +++ b/multibody/topology/spanning_forest.h @@ -587,8 +587,10 @@ class SpanningForest { // - 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) - // - either or both Links may be part of a WeldedLinksAssembly; it is the - // mass properties of the whole assembly that determines masslessness. + // - "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 diff --git a/multibody/topology/spanning_forest_debug.cc b/multibody/topology/spanning_forest_debug.cc index d7b2fabbcbda..6f832a0696d5 100644 --- a/multibody/topology/spanning_forest_debug.cc +++ b/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)); diff --git a/multibody/topology/test/spanning_forest_test.cc b/multibody/topology/test/spanning_forest_test.cc index 36ce834c5fe6..121a9e05c24f 100644 --- a/multibody/topology/test/spanning_forest_test.cc +++ b/multibody/topology/test/spanning_forest_test.cc @@ -2144,6 +2144,62 @@ GTEST_TEST(SpanningForest, LoopClosingOnWorldDoesNotSplitWorld) { } } +/* Same topology as LoopClosingOnWorldDoesNotSplitWorld above, except that the +Link we are forced to split is itself massless even though it has been fused +into the (very massful) World WeldedLinksAssembly: + + {0}==={1}==={2*} (welds, all fused onto the World Mobod) + ^ | + +---revolute---+ (a loop joint, closing back onto World) + +A shadow gets a share of its primary Link's _own_ mass properties, not those of +the whole assembly the primary belongs to. So splitting {2*} here produces a +massless, articulated, terminal shadow Mobod, i.e. a singular mass matrix. We +should still build the kinematic forest (and still not split World) but must +report that this model can't be used for dynamics. Try both directions for the +loop joint since the outcome shouldn't depend on which end the user called the +parent. */ +GTEST_TEST(SpanningForest, LoopClosingOnWorldOntoMasslessLinkKillsDynamics) { + for (bool world_is_parent : {false, true}) { + SCOPED_TRACE(fmt::format("world_is_parent={}", world_is_parent)); + LinkJointGraph graph; + graph.RegisterJointType("revolute", 1, 1); + graph.SetGlobalForestBuildingOptions( + ForestBuildingOptions::kFuseWeldedLinksAssemblies); + graph.AddLink("link1", default_model_instance()); + graph.AddLink("link2", default_model_instance(), LinkFlags::kMassless); + graph.AddJoint("weld0", default_model_instance(), "weld", LinkIndex(0), + LinkIndex(1)); + graph.AddJoint("weld1", default_model_instance(), "weld", LinkIndex(1), + LinkIndex(2)); + graph.AddJoint("revolute", default_model_instance(), "revolute", + world_is_parent ? LinkIndex(0) : LinkIndex(2), + world_is_parent ? LinkIndex(2) : LinkIndex(0)); + + EXPECT_FALSE(graph.BuildForest()); // Dynamics is not OK. + + /* Once fused, link2 is "effectively massful" as far as the assembly-aware + predicate is concerned (it's welded to World!), but its shadow isn't. */ + EXPECT_TRUE(graph.link_by_index(LinkIndex(2)).is_massless()); + EXPECT_FALSE(graph.link_and_its_assembly_are_massless(LinkOrdinal(2))); + + EXPECT_THAT(graph.forest().why_no_dynamics(), + testing::MatchesRegex("Loop breaks.*revolute.*between World " + "and massless link link2.*World can't be " + "split.*cannot be used for dynamics.*")); + + /* We split link2, not World, just as in the massful case. */ + EXPECT_EQ(ssize(graph.links()), 4); // One shadow link was added. + EXPECT_EQ(graph.world_link().num_shadows(), 0); + EXPECT_EQ(graph.link_by_index(LinkIndex(2)).num_shadows(), 1); + const LinkJointGraph::Link& shadow = graph.link_by_index(LinkIndex(3)); + EXPECT_TRUE(shadow.is_shadow()); + EXPECT_EQ(shadow.primary_link(), LinkIndex(2)); + EXPECT_EQ(ssize(graph.forest().mobods()), 2); + EXPECT_EQ(graph.link_to_mobod(LinkIndex(3)), MobodIndex(1)); + } +} + /* WeldedLinksAssemblies should be treated the same as single bodies while building the trees a level at a time. We'll create a loop out of two trees, one composed of two-body assemblies and the other single bodies. From d346b56a98fab59d8803a799966cd3db878f1e3d Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Wed, 2 Sep 2026 14:46:11 -0700 Subject: [PATCH 3/3] Respond to Sean's review --- multibody/topology/spanning_forest.cc | 23 +++++++++--------- multibody/topology/spanning_forest.h | 18 ++++++++------ .../topology/test/spanning_forest_test.cc | 24 +++++++++---------- 3 files changed, 35 insertions(+), 30 deletions(-) diff --git a/multibody/topology/spanning_forest.cc b/multibody/topology/spanning_forest.cc index 3b6ab8eff01f..836ed38354d9 100644 --- a/multibody/topology/spanning_forest.cc +++ b/multibody/topology/spanning_forest.cc @@ -923,19 +923,20 @@ void SpanningForest::HandleLoopClosure(JointOrdinal loop_joint_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 at joint {} between World and massless 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", - loop_joint.name(), split_link.name(), split_link.name()); + "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 at joint {} between two massless links {} and {}. " - "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", - loop_joint.name(), parent_link.name(), child_link.name()); + "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()); } } diff --git a/multibody/topology/spanning_forest.h b/multibody/topology/spanning_forest.h index 8055fac47f55..519c53f2a8f1 100644 --- a/multibody/topology/spanning_forest.h +++ b/multibody/topology/spanning_forest.h @@ -96,13 +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. 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. +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 diff --git a/multibody/topology/test/spanning_forest_test.cc b/multibody/topology/test/spanning_forest_test.cc index 121a9e05c24f..ee21cfc18455 100644 --- a/multibody/topology/test/spanning_forest_test.cc +++ b/multibody/topology/test/spanning_forest_test.cc @@ -2026,8 +2026,9 @@ GTEST_TEST(SpanningForest, MasslessLoopAreDetected) { EXPECT_FALSE(graph.BuildForest()); EXPECT_THAT( graph.forest().why_no_dynamics(), - testing::MatchesRegex("Loop breaks.*joint1.*between two massless links.*" - "link4.*link3.*cannot be used for dynamics.*")); + testing::MatchesRegex("Loop breaks between two massless links.*" + "link4.*link3.*joint1 retargeted.*cannot be used " + "for dynamics.*")); graph.ChangeLinkFlags(LinkIndex(1), LinkFlags::kDefault); // Massful now. EXPECT_TRUE(graph.BuildForest()); @@ -2059,9 +2060,10 @@ GTEST_TEST(SpanningForest, MasslessLoopAreDetected) { this model can't be used for dynamics. */ EXPECT_FALSE(world_graph.BuildForest()); EXPECT_THAT(world_graph.forest().why_no_dynamics(), - testing::MatchesRegex("Loop breaks.*joint1.*between World and " - "massless link link3.*World can't be " - "split.*cannot be used for dynamics.*")); + testing::MatchesRegex("Loop breaks between World and massless " + "link link3.*joint1 retargeted.*World " + "can't be split.*cannot be used for " + "dynamics.*")); EXPECT_EQ(world_graph.num_user_links(), 4); EXPECT_EQ(ssize(world_graph.links()), 5); EXPECT_EQ(ssize(world_graph.forest().mobods()), 5); @@ -2112,10 +2114,7 @@ GTEST_TEST(SpanningForest, LoopClosingOnWorldDoesNotSplitWorld) { /* The shadow is of link2, not World. */ const LinkJointGraph::Link& shadow = graph.link_by_index(LinkIndex(3)); - EXPECT_TRUE(shadow.is_shadow()); EXPECT_EQ(shadow.name(), "link2$1"); - EXPECT_EQ(shadow.primary_link(), LinkIndex(2)); - EXPECT_EQ(graph.link_by_index(LinkIndex(2)).num_shadows(), 1); /* World, link1, and link2 all follow the World Mobod; the shadow gets its own Mobod mobilized by the revolute joint. */ @@ -2148,7 +2147,7 @@ GTEST_TEST(SpanningForest, LoopClosingOnWorldDoesNotSplitWorld) { Link we are forced to split is itself massless even though it has been fused into the (very massful) World WeldedLinksAssembly: - {0}==={1}==={2*} (welds, all fused onto the World Mobod) + {0}===={1}===={2*} (welds, all fused onto the World Mobod) ^ | +---revolute---+ (a loop joint, closing back onto World) @@ -2184,9 +2183,10 @@ GTEST_TEST(SpanningForest, LoopClosingOnWorldOntoMasslessLinkKillsDynamics) { EXPECT_FALSE(graph.link_and_its_assembly_are_massless(LinkOrdinal(2))); EXPECT_THAT(graph.forest().why_no_dynamics(), - testing::MatchesRegex("Loop breaks.*revolute.*between World " - "and massless link link2.*World can't be " - "split.*cannot be used for dynamics.*")); + testing::MatchesRegex("Loop breaks between World and massless " + "link link2.*revolute retargeted " + ".*World can't be split.*cannot be used " + "for dynamics.*")); /* We split link2, not World, just as in the massful case. */ EXPECT_EQ(ssize(graph.links()), 4); // One shadow link was added.