[multibody] Prevent multibody/topology from splitting World to break a loop. by sherm1 · Pull Request #24909 · RobotLocomotion/drake

sherm1 · 2026-08-20T23:57:05Z

Previously there were odd circumstances in which the topology code would decide to break a loop by splitting off an arbitrary-mass "shadow" link from World (and welding it back on). That's never a good idea and isn't supported by the implementation in #24864 since shadow links can't have their own mass properties (they get mass from their primary links). @SeanCurtis-TRI and his trusty AI noticed this problem while reviewing #24864.

This PR modifies the loop breaking algorithm slightly to prevent ever choosing World as the to-be-broken link, modifies an existing test case to reflect the new expectation that World won't be broken, and adds a new test case in which fusing links to weld and then connecting that fused assembly back to World would decide to split World in some circumstances.

A few comments are updated to note that World is never split.

There are no user-visible changes since automatic loop breaking isn't yet enabled in Drake. No release notes are required.

This change is

sherm1

+a:@SeanCurtis-TRI for feature review, please (not a rush)

@sherm1 made 1 comment.
Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers (waiting on SeanCurtis-TRI).

Copilot

Pull request overview

Prevents loop breaking from creating shadow links for World while preserving valid topology construction.

Changes:

Prioritizes splitting the non-World link during loop closure.
Reports dynamics failures when this forces splitting a massless link.
Updates documentation and adds regression coverage.

Reviewed changes

Copilot reviewed 4 out of 4 changed files in this pull request and generated no comments.

File	Description
`multibody/topology/spanning_forest.cc`	Implements World-safe loop splitting.
`multibody/topology/spanning_forest.h`	Documents loop-breaking guarantees.
`multibody/topology/link_joint_graph.h`	Clarifies that World cannot have shadows.
`multibody/topology/test/spanning_forest_test.cc`	Updates expectations and adds regression tests.

💡 Add a code-review agent skill or configure MCP servers for context-aware, tailored reviews. Learn more in the docs.

sherm1 · 2026-08-21T23:10:25Z

Here's a review from ChatGPT

Overall verdict

I reviewed the PR head at commit 9205ba9. I would request changes for one correctness issue. The new World-first selection rule itself looks sound, and the massful regression usefully covers both joint orientations, but the code conflates:

whether an existing welded assembly is massless, and
whether a new shadow of one individual link will have any inertia.

Those are not equivalent under the shadow-mass implementation this PR is intended to support. ([GitHub]1)

1. Major: `dynamics_ok` can remain true for a zero-mass shadow

In SpanningForest::HandleLoopClosure(), the new code computes parent_is_massless and child_is_massless using link_and_its_assembly_are_massless(). It then reuses those values to decide whether the selected split link will produce a massless shadow. ([GitHub]2)

That helper intentionally reports a kMassless link as effectively massful when it belongs to a massful welded assembly. An assembly containing World is considered massful, even when a particular follower link in that assembly is itself massless. Drake separately exposes Link::is_massless() as the individual-link property. ([GitHub]3)

However, the shadow-mass work in #24864 divides the inertia of the individual primary link, not the inertia of its fused Mobod or entire welded assembly. Its implementation discussion explicitly distinguishes splitting individual links from splitting Mobods containing multiple fused links. ([GitHub]4)

A reproducer is almost identical to the new test:

World --weld-- link1 --weld-- link2 (kMassless)
   \---------------- revolute ----------------/

With welded-link fusion:

link2 belongs to the massful World assembly.
link_and_its_assembly_are_massless(link2) therefore returns false.
The new World rule correctly chooses link2, not World, as the split link.
split_link_is_massless is also false because it uses the assembly-aware result.
The forest is reported dynamics-capable.
The shadow receives a share of link2’s own zero inertia, leaving a massless terminal shadow—the condition the diagnostic is supposed to reject.

So the new check can produce a false-positive dynamics_ok. The same predicate mismatch can also affect non-World loops when a massless link is hidden inside a massful welded assembly.

Suggested fix

Use the prospective shadow’s individual-link masslessness for both candidate selection and the final validity check:

const bool parent_shadow_would_be_massless =
    links(parent_ordinal).is_massless();
const bool child_shadow_would_be_massless =
    links(child_ordinal).is_massless();

bool split_parent = false;
if (parent_is_world || child_is_world) {
  split_parent = child_is_world;
} else if (!parent_shadow_would_be_massless &&
           !child_shadow_would_be_massless) {
  const int parent_level = mobods(parent_mobod).level();
  const int child_level = mobods(child_mobod).level();
  split_parent = parent_level > child_level;
} else if (child_shadow_would_be_massless) {
  // Split the individually massful endpoint.
  split_parent = true;
}

const bool split_link_is_massless =
    split_parent ? parent_shadow_would_be_massless
                 : child_shadow_would_be_massless;

If the assembly-aware values remain necessary for another aspect of branch construction, retain both sets of booleans with names that make the distinction explicit, such as:

parent_assembly_is_massless
parent_shadow_would_be_massless

The accompanying HandleLoopClosure() documentation should also be revised. Its statement that the whole welded assembly determines masslessness is appropriate for the existing primary Mobod, but not for the newly created shadow, whose mass comes from the selected link. ([GitHub]2)

Regression test to add

Duplicate or parameterize LoopClosingOnWorldDoesNotSplitWorld, make link2 individually massless while leaving it in the massful World assembly, and retain both values of world_is_parent:

graph.AddLink(
    "link2", default_model_instance(), LinkFlags::kMassless);

// Same welded World assembly and loop-closing revolute joint.

EXPECT_FALSE(graph.BuildForest());
EXPECT_EQ(graph.world_link().num_shadows(), 0);
EXPECT_EQ(graph.link_by_index(LinkIndex(2)).num_shadows(), 1);
EXPECT_THAT(graph.forest().why_no_dynamics(),
            testing::HasSubstr("massless"));

The expected result is: kinematic forest constructed, World never shadowed, but dynamics marked unavailable.

2. Minor: enforce “World may not be shadowed” at the mutation boundary

The PR documents LinkJointGraph::AddShadowLink() with a precondition that the primary link is not World, but the implementation itself does not enforce that condition. The assertion currently lives one layer higher in SpanningForest::AddShadowMobod(). Because AddShadowLink() is the operation that actually mutates the graph, it is the strongest place to protect the invariant. ([GitHub]2)

I suggest adding:

const Link& primary_link = links(primary_link_ordinal);
DRAKE_DEMAND(!primary_link.is_world());

directly inside LinkJointGraph::AddShadowLink().

It would also be useful for SanityCheckForest() to explicitly verify that World has zero shadows. The checker already verifies several special properties of World, so this would make the newly documented invariant executable rather than relying only on the construction path. ([GitHub]5)

DRAKE_THROW_UNLESS(graph().world_link().num_shadows() == 0);

This is defense in depth rather than a current production-path bug, since the present caller is guarded.

3. Minor: complete the orientation coverage for the massless case

The new massful/fused-World test exercises both world_is_parent values, which is good. The modified massless-loop test covers only the orientation where World is the parent. ([GitHub]2)

The massless-in-a-massful-assembly regression above should run in both orientations. That directly covers:

the new World-precedence branch,
joint reversal when the parent is selected,
the dynamics diagnostic,
and the no-World-shadow invariant.

What looks good

The basic World precedence is clear and correct: whenever exactly one loop-closing endpoint is World, the other endpoint is selected, independently of Mobod depth or masslessness. The added massful regression verifies both joint directions, the active-joint bookkeeping, the loop constraint, and the resulting shadow count. ([GitHub]2)

The change also improves the diagnostic structure by checking the link actually selected for splitting instead of only recognizing the “both endpoints massless” case. Once the per-link versus per-assembly predicate is corrected, that organization should handle the World-plus-massless case cleanly. ([GitHub]2)

One readability polish would be to write:

!parent_is_massless && !child_is_massless

rather than:

!(child_is_massless || parent_is_massless)

The former directly matches the accompanying “both massful” comment.

Validation limitation

This was a source-level review; I was not able to execute Drake’s test suite in this environment. At the time examined, GitHub displayed zero checks attached to the PR, so compile and test status were not independently confirmed. ([GitHub]2)

SeanCurtis-TRI

It's basically, but I'm delaying it slightly to see what you think about some of my comments. See below.

@SeanCurtis-TRI reviewed 4 files and all commit messages, and made 7 comments.
Reviewable status: 6 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers (waiting on sherm1).

multibody/topology/spanning_forest.h line 102 at r2 (raw file):

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

nit: I'm not sure what this term means. It is first introduced in this PR.

My primary concern with the phrase is that it is so ambiguous. IN a loop, arguably every joint is a loop closing joint. However, I'm sure that's not what you mean. What you mean is that when your heuristic has decided on targeting one of those joints, it becomes a "loop-closing joint".

I don't mind the introduction of the term, but it should be defined somewhere. In an ideal world, the user should be able to look at a model with a loop and reliably predict which joint is the closing joint. Know what I mean?

It seems particularly significant because the loop could suddenly make the system incompatible with dynamics. Should I feel confident that that outcome is purely a function of the model? Or if the heuristic were slightly tweaked, could it have classified a different joint as the loop-closing joint and I wouldn't have ended up with a massless leaf?

Code quote:

loop-closing Joint

multibody/topology/spanning_forest.h line 584 at r2 (raw file):

  // forward or reversed Mobilizer of the Joint's type. Then we add a Weld
  // Constraint to attach the shadow to its primary. Some details:
  //  - we never split World, so if one of the Links is World we must split

BTW From this documentation, I infer that the "loop-closing" joint is simply the joint we get that appears to connect the leaves of two trees. The joint that is, in some sense, "equidistant" to the world. If that is wrong, ignore what follows...

As documented above, this can lead to the possibility of splitting a massless body and making the tree unacceptable for dynamics, even if the the loop is otherwise acceptable if only we'd split a different body in the loop.

This seems like a defect in the approach. It converts a valid model into an "invalid" model based on something outside the control of the model author. Surely, it would be better to walk back up the two trees being connected and find the nearest joint that has a massful body for splitting. Sure, we have to surrender how balanced the forest is, but an unbalanced forest that can actually evaluate dynamics is clearly superior to a balanced forest that can't.

multibody/topology/spanning_forest.cc line 871 at r2 (raw file):

  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));

BTW This seems like an incredibly conservative test. I'd posit that this would be better as part of LinkJointGraph::Joint (or earlier) where the Joint class asserts that the parent and child links can't be the same. Making it an enforced invariant on the Joint removes the responsibility of the spanning algorithm to worry about how the graph is specified.

On the other hand, is this the only case in which we care about the property that the links are different? And by deferring the test here, we only pay it if we actually have to handle a loop?

multibody/topology/spanning_forest.cc line 925 at r2 (raw file):

    } else {
      data_.why_no_dynamics = fmt::format(
          "Loop breaks at joint {} between two massless links {} and {}. "

BTW Both error messages seem to use misleading language: "the loop breaks at joint". Obviously, the model doesn't break any joints. This is Drake apparently doing the breaking. But Drake isn't breaking joints. Given that "breaking a joint" to resolve loops is a strong term of art, it seems inadvisable to use it here when we're not actually breaking joints.

I'm not sure what the preferred language would be, I'd have to ponder that.

multibody/topology/test/spanning_forest_test.cc line 2114 at r2 (raw file):

    /* The shadow is of link2, not World. */
    const LinkJointGraph::Link& shadow = graph.link_by_index(LinkIndex(3));

BTW This test is about the logic for picking the body to split. The mechanism for creating the shadow has already been well tested. As such, instead of checking for every aspect that suggests the shadow is what it says it is, it's probably enough to check for one property and call it good. Checking the shadow name would be sufficient (all other properties have been tested as correlated with the name).

multibody/topology/test/spanning_forest_test.cc line 2120 at r2 (raw file):

    EXPECT_EQ(graph.link_by_index(LinkIndex(2)).num_shadows(), 1);

    /* World, link1, and link2 all follow the World Mobod; the shadow gets its

nit: Related to the previous note, all of the tests below here are correlated with having not picked any of the other links to split. So, is it necessary to explicitly test everything?

sherm1

All review comments addressed, PTAL.

I put the response to Claude's comments in one commit and the response to yours (Sean's) in the next commit. Probably easier just to review them together though.

@sherm1 made 7 comments and resolved 6 discussions.
Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on SeanCurtis-TRI).

multibody/topology/spanning_forest.h line 102 at r2 (raw file):