[multibody] Calc fused (née composite) mass properties

[mitiguy]

Branched to continue work from Sherm's PR #24630 (now closed).

Adds an (Internal use only) option to turn on the topology feature where welded-together links are combined into a fused mobilized body. The only thing done with that here is to compute the fused body's spatial inertia and check that it is correct. The only substantive change is in multibody_tree.cc where we are filling in the FrameBodyPoseCache (that's only done when parameters change so isn't performance critical).

Also cleans up a few related comments.

There is no end-user value here yet. This is a separately-reviewable piece of the larger fused-body project #24350.

Note: we are going to change terminology from the ambiguous "composite" to Ganesh's brilliant "fused" term. However we don't want to mix that with these substantive changes so that change will be in a follow up PR.

---

This change is 

[Copilot]

Pull request overview

Adds an internal (for now) configuration switch to combine welded-together links into a single composite / fused mobilized body and extends the tree caches and mass-properties computations to correctly support composite mobods. This enables verifying fused-body spatial inertias and fixes several pre-existing edge cases (e.g., links welded to world and per-link spatial inertia queries under composite mode).

Changes:

• Adds SetCombineWeldedBodies / GetCombineWeldedBodies to MultibodyPlant and MultibodyTree, backed by ForestBuildingOptions.
• Extends CalcFrameBodyPoses / kinematics caches to compute X_BL, X_WL, and composite mobod mass properties by accumulating follower-link inertias.
• Adds composite_test.cc to validate mass properties and exercise normal + reversed weld code paths; plus related doc/comment cleanups.

Reviewed changes

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

Show a summary per file

File	Description
multibody/tree/multibody_tree.h	Declares combine-welded-bodies getters/setters; clarifies pose-cache role of CalcFrameBodyPoses.
multibody/tree/multibody_tree.cc	Implements combine flag; updates frame/body pose + mass-property caching for composites; fixes composite edge cases in CalcSpatialInertia and related paths.
multibody/tree/multibody_forces.h	Updates force semantics/docs toward mobod-based indexing (documentation fixes still needed per inline comments).
multibody/tree/multibody_forces.cc	Switches force storage sizing from num_links() to num_mobods().
multibody/tree/linear_bushing_roll_pitch_yaw.h	Fixes a doc typo (“physical” → “physically”).
multibody/tree/frame.h	Minor doc reflow.
multibody/tree/frame_body_pose_cache.h	Adds AddToM_BBo_B helper for composite inertia accumulation.
multibody/tree/force_element.h	Updates docs to reflect link-vs-mobod force application responsibilities under composites.
multibody/tree/body_node_impl.cc	Sets X_WL for follower links in composite mobods (and for world composites).
multibody/plant/test/composite_test.cc	New tests covering composite inertias, CalcSpatialInertia behavior, and full CalcFrameBodyPoses paths (normal + reversed weld).
multibody/plant/multibody_plant.h	Adds internal API docs + declarations for combine-welded-bodies option.
multibody/plant/multibody_plant.cc	Wires combine-welded-bodies option through to the internal tree.
multibody/plant/BUILD.bazel	Registers the new composite_test.
bindings/generated_docstrings/multibody_tree.h	Regenerates docstrings reflecting updated comments/docs.
bindings/generated_docstrings/multibody_plant.h	Regenerates docstrings for new plant APIs and doc tweaks.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[sherm1]

Done

[mitiguy]

Done. Seems resolved.

[sherm1]

Done

[sherm1]

Done

[sherm1]

+@mitiguy for freature review of Sherm's changes
+@sherm1 for feature review of Paul's changes

@sherm1 made 1 comment.
Reviewable status: 4 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on mitiguy).

[sherm1]

Feature with a few minor comments

@sherm1 reviewed 17 files and all commit messages, made 14 comments, and resolved 3 discussions.
Reviewable status: 11 unresolved discussions, LGTM missing from assignee mitiguy, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on mitiguy).

---

multibody/plant/test/composite_test.cc line 173 at r2 (raw file):

frame): Link1: p₁ = (0, 0, 0)   — at the joint Link2: p₂ = (1, 0, 0)   in
Link1's frame Link3: p₃ = (1, 1, 0)   in Link1's frame (1 m in x then 1 m in
Link2's y)

minor: odd formatting of the above 3 lines

---

multibody/plant/test/composite_test.cc line 206 at r2 (raw file):

    SetState(composite_model, angle, 0.0);

    // Verify Link123's spatial inertia not depend on combined welded links.

BTW consider

Suggestion:

Verify Link123's combined spatial inertia doesn't depend on whether they are combined.

---

multibody/plant/test/composite_test.cc line 207 at r2 (raw file):

    // Verify Link123's spatial inertia not depend on combined welded links.
    // Note: Due to Bug 1 above, the following test used to fail (now fixed)

minor: Bug 1 is first mentioned below

---

multibody/plant/test/composite_test.cc line 226 at r2 (raw file):

    // the entire Link123 composite mobod. Bug 2: When CalcSpatialInertia()
    // was called with composite_model on link4 (which is welded to world), an
    // exception was thrown (debug mode) or NaN were returned (release mode).

minor: since these bugs are gone (so not relevant now), reword this to say what's being tested here, e.g. "Ensure that individual link spatial inertias are reported correctly regardless of whether they were fused."

---

multibody/plant/test/composite_test.cc line 241 at r2 (raw file):

      // Due to special case of link4 being welded to world, compare link4's
      // spatial inertia to its expected value.

minor: this i==3 block seems unnecessary now?

---

multibody/plant/test/composite_test.cc line 283 at r2 (raw file):

general, X_BL is non-identity for each "follower" link L welded into the
composite body B.
- Pass 3: mass property accumulation with both shifting and re-expressing.

minor: above needs consistent formatting

---

multibody/tree/multibody_forces.cc line 45 at r2 (raw file):

    const internal::MultibodyTree<T>& model) const {
  return model.num_velocities() == num_velocities() &&
         model.num_mobods() == num_bodies();

minor

Suggestion:

model.num_mobods() == num_mobods();

---

multibody/tree/multibody_tree.cc line 1563 at r2 (raw file):

  // TODO(joemasterjohn): Consider an optimization to avoid calculating spatial
  //  inertias for locked floating bodies.
  for (LinkIndex link_index(1); link_index < num_links(); ++link_index) {

BTW since this loop is calculating just the mobod's M_BBo_W it seems wrong to run through the links here rather than the mobods. For fused mobods this will recalculate the same M_BBo_W repeatedly for each follower link. Would still end up with the right answer, just computed in a dumb way.

---

multibody/tree/multibody_tree.cc line 2571 at r2 (raw file):

      EvalSpatialInertiaInWorldCache(context);
  const PositionKinematicsCache<T>& pc = EvalPositionKinematics(context);
  const FrameBodyPoseCache<T>& frame_body_poses = EvalFrameBodyPoses(context);

BTW eval the frame body pose cache before the position kinematics cache (by convention)

---

multibody/tree/multibody_tree.cc line 2600 at r2 (raw file):

      // For a composite body (having more than one link), need to calculate
      // the individual link's spatial inertia about Wo expressed in W since
      // there is no cached value (and perhaps not need for one).

typo: not -> no

[sherm1]

Done

[sherm1]

Done

[sherm1]

Done

[mitiguy]

Check-point. Need to finish
multibody_tree.cc restart at line 1563
position_kinematics_cache.h
position_kinematics_cache.cc

@mitiguy reviewed 12 files and all commit messages, made 9 comments, and resolved 7 discussions.
Reviewable status: 4 unresolved discussions, LGTM missing from assignee mitiguy, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on sherm1).

---

multibody/plant/test/composite_test.cc line 173 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: odd formatting of the above 3 lines

Done.

---

multibody/plant/test/composite_test.cc line 206 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider

Done. With slightly different wording that ideally will change when we put in "fused".

---

multibody/plant/test/composite_test.cc line 207 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: Bug 1 is first mentioned below

Done. This line was remove.

---

multibody/plant/test/composite_test.cc line 226 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: since these bugs are gone (so not relevant now), reword this to say what's being tested here, e.g. "Ensure that individual link spatial inertias are reported correctly regardless of whether they were fused."

Done.

---

multibody/plant/test/composite_test.cc line 241 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: this i==3 block seems unnecessary now?

Updated comment.

---

multibody/plant/test/composite_test.cc line 283 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: above needs consistent formatting

Done.

---

multibody/tree/multibody_forces.cc line 45 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor

Done.

[mitiguy]

Done. Seems resolved.

[sherm1]

@sherm1 reviewed 1 file and resolved 1 discussion.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee mitiguy, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on mitiguy).

[sherm1]

@sherm1 made 3 comments and resolved 3 discussions.
Reviewable status: LGTM missing from assignee mitiguy (waiting on mitiguy).

---

multibody/tree/multibody_tree.cc line 1563 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW since this loop is calculating just the mobod's M_BBo_W it seems wrong to run through the links here rather than the mobods. For fused mobods this will recalculate the same M_BBo_W repeatedly for each follower link. Would still end up with the right answer, just computed in a dumb way.

Done

---

multibody/tree/multibody_tree.cc line 2571 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW eval the frame body pose cache before the position kinematics cache (by convention)

Done

---

multibody/tree/multibody_tree.cc line 2600 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

typo: not -> no

Done

[sherm1]

@sherm1 reviewed 1 file and all commit messages.
Reviewable status: LGTM missing from assignee mitiguy (waiting on mitiguy).

[mitiguy]

@mitiguy made 2 comments.
Reviewable status: 1 unresolved discussion (waiting on sherm1).

---

multibody/tree/multibody_tree.cc line 1701 at r4 (raw file):

        const math::RigidTransformd& X_JpJc = weld_joint.X_FM();
        const RigidTransform<T> X_JiJo =
            (is_reversed ? X_JpJc.inverse() : X_JpJc).cast<T>();

nit Add a comment and/or change "d" to "" to clarify why a cast to is needed later.

Suggestion:

        const math::RigidTransform<double>& X_JpJc = weld_joint.X_FM();
        const RigidTransform<T> X_JiJo =
            (is_reversed ? X_JpJc.inverse() : X_JpJc).cast<T>();

[sherm1]

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

@sherm1 made 1 comment.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee xuchen-han(platform) (waiting on mitiguy and xuchen-han).

[xuchen-han]

pending CI

@xuchen-han reviewed 17 files and all commit messages, and made 2 comments.
Reviewable status: 2 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on mitiguy and sherm1).

---

multibody/tree/multibody_forces.cc line 45 at r2 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Done.

This doesn't build. MultibodyForces doesn't have num_mobods().

[mitiguy]

@sherm1 -- Ugh, I should have checked the num_bodies() -> num_mobods() change in multibody_forces.cc before submitting my last commit. If you happen to see this message this morning, please revert num_mobods() to num_bodies(). Otherwise, I'll make the change later.

@mitiguy reviewed 1 file, made 2 comments, and resolved 1 discussion.
Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on sherm1).

---

multibody/tree/multibody_tree.cc line 1701 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Add a comment and/or change "d" to "" to clarify why a cast to is needed later.

Done.

[sherm1]

Done

@sherm1 partially reviewed 3 files and made 2 comments.
Reviewable status: 1 unresolved discussion (waiting on xuchen-han).

---

multibody/tree/multibody_forces.cc line 45 at r2 (raw file):

Previously, xuchen-han (Xuchen Han) wrote…

This doesn't build. MultibodyForces doesn't have num_mobods().

Done

[sherm1]

@sherm1 reviewed 1 file and all commit messages.
Reviewable status: 1 unresolved discussion (waiting on xuchen-han).

[sherm1]

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