[multibody] Auto loop breaking: retarget joints

[sherm1]

This is the second PR in the train leading up to fully-functional automatic handling of mechanisms with closed topological loops (PR #24843). The first was #24864, which handled splitting a link into primary and shadow links and distributing the mass properties. Here we handle re-targeting the chosen joint from the original link to the shadow, with added unit tests to verify that it works.

What's in this PR

• a new frame type ShadowFrame whose pose on the shadow link follows the one on the primary link (which is parameterized)
• a notion of "effective parent/child frame" for a Joint in case we had to re-target one end of it to a shadow link. The original parent/child frames remain unchanged
• additions to the auto_closed_topology_test file to add tests to verify that re-targeting works properly
• small changes to a few joints that didn't grab their frames from the standard interface.

There are no user-visible features added here (and the option that allows loop handling is "internal use only"), so no release notes.

The next PR will add the missing weld constraint that re-attaches the shadow to the primary, but that isn't done here so the test mechanism can't actually be simulated yet.

---

This change is 

[sherm1]

+a:@joemasterjohn for feature review (per f2f the other day)
Note that this is only 500 lines (second commit). The rest is just a rebase onto an in-progress PR and doesn't need review here.
You might want to start with multibody_tree.cc to see how Finalize() re-targets the Joint before wading through the test cases which come up first in alphabetical order.

@sherm1 made 1 comment.
Reviewable status: LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn).

[Copilot]

Pull request overview

Adds joint retargeting to shadow links as part of automatic closed-loop topology handling.

Changes:

• Introduces ShadowFrame and effective joint frames.
• Updates mobilizers, cloning, mass splitting, and geometry bookkeeping.
• Adds four-bar loop and scalar-conversion tests.

Reviewed changes

Copilot reviewed 19 out of 19 changed files in this pull request and generated 2 comments.

Show a summary per file

File	Description
multibody/tree/weld_joint.cc	Uses effective joint frames.
multibody/tree/shadow_frame.h	Declares shadow frame support.
multibody/tree/shadow_frame.cc	Implements delegation and cloning.
multibody/tree/rigid_body.h	Adds split-inertia access and guards.
multibody/tree/rigid_body.cc	Handles ephemeral inertia parameters.
multibody/tree/revolute_joint.cc	Uses effective frames.
multibody/tree/prismatic_joint.cc	Uses effective frames.
multibody/tree/multibody_tree.h	Declares loop-topology infrastructure.
multibody/tree/multibody_tree.cc	Materializes shadows and retargets joints.
multibody/tree/joint.h	Adds effective frame substitution.
multibody/tree/BUILD.bazel	Builds shadow frame sources.
multibody/topology/link_joint_graph_link.h	Exposes retargeted-joint mapping.
multibody/plant/test/multibody_plant_test.cc	Tests loop option plumbing.
multibody/plant/test/auto_closed_topology_test.cc	Tests shadows and retargeting.
multibody/plant/multibody_plant.h	Adds loop-topology API.
multibody/plant/multibody_plant.cc	Implements API and geometry resizing.
multibody/plant/BUILD.bazel	Registers closed-topology tests.
bindings/generated_docstrings/multibody_tree.h	Updates generated tree documentation.
bindings/generated_docstrings/multibody_plant.h	Updates generated plant documentation.

---

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

[sherm1]

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

[sherm1]

@joemasterjohn the first commit merged so I have rebased this one. Should be easier to review now.

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

[joemasterjohn]

@joemasterjohn reviewed 16 files and all commit messages, and made 11 comments.
Reviewable status: 9 unresolved discussions, LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on sherm1).

---

multibody/plant/test/auto_closed_topology_test.cc line 240 at r5 (raw file):

  // the shadow link's own link frame, and a joint frame that was originally
  // on the primary link. A mobilizer implementing the joint may also add
  // an ephemeral frame of its own, but doesn't in this case.

Claude pointed out something interesting. One revolute joint (I believe it is coupler_rocker, if the coupler is the body that gets split) will be reversed, making its axis -y. In which case the revolute joint will add ephemeral M and F frames when creating the mobilizer. Heh.

Code quote:

but doesn't in this case.

---

multibody/plant/test/auto_closed_topology_test.cc line 606 at r5 (raw file):

  }
  EXPECT_EQ(found.size(), 1);
  return found.empty() ? RetargetedJoint{} : found.front();

Make this an ASSERT here and fail fast in this function. That way the test below doesn't have to use null pointer semantics immediately after to fail.

Suggestion:

  ASSERT_EQ(found.size(), 1);
  return found.front();

---

multibody/tree/joint.h line 773 at r5 (raw file):

  // user link onto one of that link's ephemeral shadow links, MultibodyTree
  // calls one of these (before Build()) with a substitute frame fixed to the
  // shadow and coincident with the user's frame on the primary link. The

nit:

Suggestion:

coincident with the user's frame on the primary link (when the system is in a perfectly assembled state)..

---

multibody/tree/multibody_tree.cc line 1113 at r5 (raw file):

    the primary link onto this shadow. That joint's frame for that end was
    authored on the primary, so we give the joint a substitute frame that is
    fixed to the shadow and coincident with the user's frame. We can use a

nit: I'm worried about saying "coincident" here. Because in any unassembled state, the frames on the shadow and primary link are not coincident. And even when SAP/ICF converges, the weld constraint is only satisfied up to tolerances, how stiff the weld constraint is, and other competing constraints. Maybe stating that they are coincident, but only in a perfectly assembled state?

Code quote:

 coincident with the user's frame.

---

multibody/tree/multibody_tree.cc line 1114 at r5 (raw file):

    authored on the primary, so we give the joint a substitute frame that is
    fixed to the shadow and coincident with the user's frame. We can use a
    coincident frame because a shadow's link frame coincides with its

nit: grammar?

Suggestion:

shadow link's

---

multibody/tree/multibody_tree.cc line 1123 at r5 (raw file):

    frame_on_parent()/ frame_on_child() and parent_body()/child_body() are
    unchanged. */
    const LinkJointGraph::Link& primary = graph.link_by_index(primary_index);

This is a duplicate of the local primary_link from above.

Code quote:

    const LinkJointGraph::Link& primary = graph.link_by_index(primary_index);

---

multibody/tree/multibody_tree.cc line 1130 at r5 (raw file):

    // The retargeted joints are ordered to match the shadow links.
    const JointIndex joint_index = primary.joints_moved_to_shadow_links().at(
        shadow_iter - shadows.begin());

Doesn't the shadow link store its inboard joint index?

Suggestion:

    const JointIndex joint_index = graph_link.inboard_joint_index();

---

multibody/tree/multibody_tree.cc line 1139 at r5 (raw file):

        graph_joint.effective_child_link_index() == graph_link.index();
    DRAKE_DEMAND(moved_child || graph_joint.effective_parent_link_index() ==
                                    graph_link.index());

Again, I think the shadow link directly stores this info. Also, can we rename graph_link to shadow_link? Or graph_shadow_link?

Suggestion:

const bool moved_child = !graph_link.joints_as_child().empty();

---

multibody/tree/prismatic_joint.cc line 130 at r5 (raw file):

  // reversal to locate them on the inboard and outboard bodies. We may also
  // need to reverse the axis so that q will retain its expected sign.
  const auto [Jin, Jout] = this->tree_frames(reverse);

nit Jin and Jout seem like awkward names for these frames. I see the other joint implementations use inboard_frame and outboard_frame.

Code quote:

const auto [Jin, Jout]

---

multibody/tree/prismatic_joint.cc line 130 at r5 (raw file):

  // reversal to locate them on the inboard and outboard bodies. We may also
  // need to reverse the axis so that q will retain its expected sign.
  const auto [Jin, Jout] = this->tree_frames(reverse);

I see only prismatic and revolute joints are changed here. I didn't look closely, but are all other joint implementations already using tree_frames()?

Code quote:

  const auto [Jin, Jout] = this->tree_frames(reverse);

---

multibody/tree/shadow_frame.h line 24 at r5 (raw file):

the shadow link. Because a shadow's link frame coincides with its primary's,
"equivalent" just means coincident: only the link the frame is fixed to
differs, not its pose.

I guess this whole discussion confuses me a bit, because the frame on the shadow body and the original frame are only coincident when the system is perfectly assembled and the weld constraint has 0 error. In general that won't be the case. Even if SAP/ICF solves for the velocities with a really tight tolerance, integrating the positions will introduce slight error that will leave these frames non-coincident. I guess I'm fuzzy on what the implications upstream for reporting kinematics will be if this ShadowFrame makes this assumption. Sorry to delay further, but I think I need a f2f to discuss this.

Code quote:

/* A Frame fixed to an ephemeral shadow link, coincident at all times with a
"source" Frame fixed to that shadow's primary link.

When we model a closed kinematic loop we break the loop by splitting a link
into a primary link and one or more ephemeral shadow links, retargeting one of
the loop joint's frames onto a shadow (see LinkJointGraph). The joint's frame
was authored on the primary link, so the mobilizer needs an equivalent frame on
the shadow link. Because a shadow's link frame coincides with its primary's,
"equivalent" just means coincident: only the link the frame is fixed to
differs, not its pose.

[joemasterjohn]

Checkpoint. Need to discuss f2f at our 1:1 on Tuesday a major confusion I'm having.

@joemasterjohn made 1 comment.
Reviewable status: 9 unresolved discussions, LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on sherm1).

[sherm1]

Comments addressed, PTAL.

@sherm1 made 12 comments and resolved 4 discussions.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on joemasterjohn).

---

multibody/plant/test/auto_closed_topology_test.cc line 240 at r5 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Claude pointed out something interesting. One revolute joint (I believe it is coupler_rocker, if the coupler is the body that gets split) will be reversed, making its axis -y. In which case the revolute joint will add ephemeral M and F frames when creating the mobilizer. Heh.

Yes, Claude and I had a little discussion about that!

---

multibody/plant/test/auto_closed_topology_test.cc line 606 at r5 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Make this an ASSERT here and fail fast in this function. That way the test below doesn't have to use null pointer semantics immediately after to fail.

Much nicer! Unfortunately ASSERT_EQ doesn't compile here because it is in a non-void returning function. We're stuck with EXPECT :(

---

multibody/tree/joint.h line 773 at r5 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

nit:

Done, PTAL. What I meant was to say that the new frame has the same local pose on the shadow as the original had on the primary.

---

multibody/tree/multibody_tree.cc line 1113 at r5 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

nit: I'm worried about saying "coincident" here. Because in any unassembled state, the frames on the shadow and primary link are not coincident. And even when SAP/ICF converges, the weld constraint is only satisfied up to tolerances, how stiff the weld constraint is, and other competing constraints. Maybe stating that they are coincident, but only in a perfectly assembled state?

Done. Clarified. "coincident" is the wrong word. "equiposed" would be good if it was actually a word!

---

multibody/tree/multibody_tree.cc line 1114 at r5 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

nit: grammar?

Done

---

multibody/tree/multibody_tree.cc line 1123 at r5 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

This is a duplicate of the local primary_link from above.

Done. Good catch!

---

multibody/tree/multibody_tree.cc line 1130 at r5 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Doesn't the shadow link store its inboard joint index?

OMG! Claude is too clever for its own good. Thanks.

---

multibody/tree/multibody_tree.cc line 1139 at r5 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Again, I think the shadow link directly stores this info. Also, can we rename graph_link to shadow_link? Or graph_shadow_link?

I fixed a bunch of awful names (those were my fault, not Claude's). Hopefully it's easier to read now. Still investigating the proposed change -- I'm not sure the joint_as_child etc are properly set on shadows -- they should be.

---

multibody/tree/prismatic_joint.cc line 130 at r5 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

I see only prismatic and revolute joints are changed here. I didn't look closely, but are all other joint implementations already using tree_frames()?

Weld also. The rest already used tree_frames().

---

multibody/tree/prismatic_joint.cc line 130 at r5 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

nit Jin and Jout seem like awkward names for these frames. I see the other joint implementations use inboard_frame and outboard_frame.

I think that would be too clunky here. I'm trying to mimic the typical frame notation where a frame gets a single capital letter and subscript. So the joint's parent frame is Jₚ and the joint's inboard frame is Jᵢₙ. That lets me use the frame names in the usual way like the rotation matrix R_ JᵢₙF. Clunkier in code, but that's what I was going for here.

---

multibody/tree/shadow_frame.h line 24 at r5 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

I guess this whole discussion confuses me a bit, because the frame on the shadow body and the original frame are only coincident when the system is perfectly assembled and the weld constraint has 0 error. In general that won't be the case. Even if SAP/ICF solves for the velocities with a really tight tolerance, integrating the positions will introduce slight error that will leave these frames non-coincident. I guess I'm fuzzy on what the implications upstream for reporting kinematics will be if this ShadowFrame makes this assumption. Sorry to delay further, but I think I need a f2f to discuss this.

Done, PTAL. I reworded the text here to hopefully be more clear.

[joemasterjohn]

Thanks for the revision, Sherm. feature. Sorry this didn't get in before your platform day Thursday. Maybe we could get a volunteer?

@joemasterjohn reviewed 5 files and all commit messages, made 7 comments, and resolved 5 discussions.
Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on sherm1).

---

multibody/plant/test/auto_closed_topology_test.cc line 606 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Much nicer! Unfortunately ASSERT_EQ doesn't compile here because it is in a non-void returning function. We're stuck with EXPECT :(

Ah, too bad.

---

multibody/tree/prismatic_joint.cc line 130 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

I think that would be too clunky here. I'm trying to mimic the typical frame notation where a frame gets a single capital letter and subscript. So the joint's parent frame is Jₚ and the joint's inboard frame is Jᵢₙ. That lets me use the frame names in the usual way like the rotation matrix R_ JᵢₙF. Clunkier in code, but that's what I was going for here.

Ah ok. Maybe J_in and J_out then? Up to you.

---

multibody/tree/prismatic_joint.cc line 130 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Weld also. The rest already used tree_frames().

Got it, thanks for confirming.

---

multibody/tree/shadow_frame.h line 24 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Done, PTAL. I reworded the text here to hopefully be more clear.

Thanks for the change, it reads more clearly now.

---

multibody/plant/test/auto_closed_topology_test.cc line 650 at r7 (raw file):

  ASSERT_NE(shadow_frame, nullptr);
  EXPECT_EQ(shadow_frame->body().index(), shadow.index());
  EXPECT_EQ(&shadow_frame->source_frame(), &loop_joint.frame_on_parent());

nit: why not just compare them by index()?

Code quote:

  EXPECT_EQ(&shadow_frame->source_frame(), &loop_joint.frame_on_parent());

---

multibody/plant/test/auto_closed_topology_test.cc line 676 at r7 (raw file):

    EXPECT_EQ(joint.frame_on_child().body().index(),
              joint.child_body().index());
  }

I'm not sure this is testing that these other joints are not re-targeted. These are the same invariants tested on the re-targeted loop_joint above (showing that the user view is unchanged). A better test would be to show that their "effective" frames are nullptr, or through the public API that effective_frame_on_parent() and effective_frame_on_child() are equal to frame_on_parent() and frame_on_child() respectively, no?

Code quote:

  for (const char* name : {"world_driver", "world_rocker", "driver_coupler"}) {
    const Joint<double>& joint = plant->GetJointByName(name);
    EXPECT_EQ(joint.frame_on_parent().body().index(),
              joint.parent_body().index());
    EXPECT_EQ(joint.frame_on_child().body().index(),
              joint.child_body().index());
  }

[joemasterjohn]

As far as I can tell, the CI failures look like flukes.

@joemasterjohn made 1 comment.
Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on sherm1).

[sherm1]

@drake-jenkins-bot retest this please

[sherm1]

Thanks, Joe!
+a:@xuchen-han for platform review per rotation, please

@sherm1 made 4 comments and resolved 2 discussions.
Reviewable status: LGTM missing from assignee xuchen-han(platform) (waiting on joemasterjohn and xuchen-han).

---

multibody/plant/test/auto_closed_topology_test.cc line 650 at r7 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

nit: why not just compare them by index()?

Done

---

multibody/plant/test/auto_closed_topology_test.cc line 676 at r7 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

I'm not sure this is testing that these other joints are not re-targeted. These are the same invariants tested on the re-targeted loop_joint above (showing that the user view is unchanged). A better test would be to show that their "effective" frames are nullptr, or through the public API that effective_frame_on_parent() and effective_frame_on_child() are equal to frame_on_parent() and frame_on_child() respectively, no?

Right, this isn't testing anything meaningful! GetSoleRetargetedJoint() above already verified that the effective frames match the original definition frames so I just removed this loop.

---

multibody/tree/prismatic_joint.cc line 130 at r5 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Ah ok. Maybe J_in and J_out then? Up to you.

For consistency with Jp and Jc I'll stick with Jin and Jout although it does look nicer with the underscore.

[xuchen-han]

@xuchen-han reviewed 19 files and all commit messages, and made 3 comments.
Reviewable status: 2 unresolved discussions (waiting on sherm1).

---

multibody/plant/test/auto_closed_topology_test.cc line 600 at r8 (raw file):

    const bool moved_child =
        &joint.effective_frame_on_child() != &joint.frame_on_child();
    // A joint has at most one end on any one link, so at most one of 77its ends

typo

Suggestion:

// A joint has at most one end on any one link, so at most one of its ends

---

multibody/tree/shadow_frame.cc line 32 at r8 (raw file):

      tree_clone.get_variant(source_frame_);
  auto new_frame = std::make_unique<ShadowFrame<ToScalar>>(
      this->name(), shadow_link_clone, source_frame_clone);

nit why don't we need to pass in the model instance? Ditto for the shallow clone below.

[sherm1]

Thanks, Xuchen. You found a real bug! All comments addressed and bug fixed, PTAL.

@sherm1 made 3 comments and resolved 1 discussion.
Reviewable status: 1 unresolved discussion (waiting on joemasterjohn and xuchen-han).

---

multibody/plant/test/auto_closed_topology_test.cc line 600 at r8 (raw file):

Previously, xuchen-han (Xuchen Han) wrote…

typo

Done (??)

---

multibody/tree/shadow_frame.cc line 32 at r8 (raw file):

Previously, xuchen-han (Xuchen Han) wrote…

nit why don't we need to pass in the model instance? Ditto for the shallow clone below.

Yikes! Good catch -- that's a bug. Fixed and added a unit test that fails without the fix.

[sherm1]

Uh oh -- hold off, I think I made a merge mess :( Working

@sherm1 made 1 comment.
Reviewable status: 1 unresolved discussion (waiting on joemasterjohn and xuchen-han).

[sherm1]

False alarm -- it's OK. PTAL

@sherm1 made 1 comment and resolved 1 discussion.
Reviewable status: complete! all discussions resolved, LGTM from assignees joemasterjohn,xuchen-han(platform) (waiting on joemasterjohn and xuchen-han).

[sherm1]

Arggh. Spoke too soon. Repairing.

@sherm1 made 1 comment.
Reviewable status: complete! all discussions resolved, LGTM from assignees joemasterjohn,xuchen-han(platform) (waiting on joemasterjohn and xuchen-han).

[sherm1]

@joemasterjohn please take one more quick look at this. I fixed a bug Xuchen found but also I had a small merge mess which I think I have fixed. Sorry! Please LMK if you spot anything I missed.

@sherm1 made 2 comments.
Reviewable status: complete! all discussions resolved, LGTM from assignees joemasterjohn,xuchen-han(platform) (waiting on joemasterjohn and xuchen-han).

---

multibody/tree/multibody_tree.cc line 1139 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

I fixed a bunch of awful names (those were my fault, not Claude's). Hopefully it's easier to read now. Still investigating the proposed change -- I'm not sure the joint_as_child etc are properly set on shadows -- they should be.

Done

[xuchen-han]

+(status: do not merge) I almost merged too prematurely. Feel free to merge when Joe's taken a look.

@xuchen-han reviewed 3 files and all commit messages, and made 1 comment.
Reviewable status: labeled "do not merge" (waiting on sherm1).

[joemasterjohn]

x2

@joemasterjohn reviewed 3 files and all commit messages, and made 1 comment.
Reviewable status: labeled "do not merge" (waiting on sherm1).