[multibody] Compute composite mass properties by sherm1 · Pull Request #24630 · RobotLocomotion/drake

sherm1 · 2026-06-09T23:16:58Z

Adds an (Internal use only) option to turn on the topology feature where welded-together links are combined into a composite mobilized body. The only thing done with that here is to compute the composite 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 parameter's 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 composite project #24350.

This change is

sherm1

+a:@mitiguy for feature review, please

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

Copilot

Pull request overview

This PR adds an internal-use option to enable combining welded-together links into composite mobilized bodies, and uses that topology to compute and validate composite mass properties (spatial inertia) via new unit tests.

Changes:

Adds SetCombineWeldedBodies / GetCombineWeldedBodies plumbing through MultibodyPlant → MultibodyTree to enable composite mobods.
Updates MultibodyTree::CalcFrameBodyPoses() to compute per-link X_BL for composite mobods and accumulate composite spatial inertia M_BBo_B.
Adjusts applied-force and inverse-dynamics bookkeeping to be mobod-indexed and adds new tests validating composite mass properties.

Reviewed changes

Copilot reviewed 14 out of 14 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 option accessors; clarifies CalcFrameBodyPoses caching.
multibody/tree/multibody_tree.cc	Implements combine-welded-bodies option; computes `X_BL`, `X_BF`, and composite `M_BBo_B` in `CalcFrameBodyPoses`; sizes inverse dynamics scratch by `num_mobods()`.
multibody/tree/multibody_forces.h	Updates documentation to describe mobod spatial forces and constraints around sizing.
multibody/tree/multibody_forces.cc	Changes `MultibodyForces(tree)` sizing from `num_links()` to `num_mobods()`.
multibody/tree/linear_bushing_roll_pitch_yaw.h	Fixes a grammar error in documentation.
multibody/tree/frame.h	Minor doc line-wrapping / clarity tweak.
multibody/tree/frame_body_pose_cache.h	Adds `AddToM_BBo_B()` helper to accumulate composite inertias in the cache.
multibody/tree/force_element.h	Updates docs to MultibodyPlant wording; adds note about shifting link-applied forces to mobods when composites exist.
multibody/plant/test/composite_test.cc	New tests verifying mass matrix / gravity forces match between explicit welds vs composite mobods, and that all CalcFrameBodyPoses code paths are exercised.
multibody/plant/multibody_plant.h	Adds public (internal-use) API docs for combine-welded-bodies option.
multibody/plant/multibody_plant.cc	Forwards combine-welded-bodies setters/getters to the internal tree.
multibody/plant/BUILD.bazel	Adds the new `composite_test` target.
bindings/generated_docstrings/multibody_tree.h	Regenerates bindings docs for updated MultibodyForces / ForceElement / bushing docs.
bindings/generated_docstrings/multibody_plant.h	Regenerates bindings docs for updated MultibodyPlant docs and new API.

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

sherm1 · 2026-06-10T23:56:13Z

+void MultibodyTree<T>::SetCombineWeldedBodies(
+    bool combine, std::optional<ModelInstanceIndex> model_instance) {
+  LinkJointGraph& graph = mutable_graph();


sherm1 · 2026-06-10T23:56:12Z

+  /// (Internal use only for now) Controls whether welded-together RigidBody
+  /// elements are to be combined into a single composite mobilized body in
+  /// the generated model. If so, the Weld joints will not appear in the
+  /// post-Finalize() model and there will be fewer bodies and joints in the


sherm1 · 2026-06-10T23:56:14Z

+        frame_body_poses->Set_p_BoLcm_B(link.ordinal(), p_BoLcm_B);
+      } else {
+        // X_BL is identity.
+        frame_body_poses->AddToM_BBo_B(mobod.index(), M_LLo_L);
+        frame_body_poses->Set_p_BoLcm_B(link.ordinal(), p_LoLcm_L);


sherm1 · 2026-06-10T23:56:13Z

-  /// Number of bodies and number of generalized velocities overload. This
-  /// constructor is useful for constructing the MultibodyForces structure
-  /// before a MultibodyPlant has been constructed.
+  /// (Advanced) Number of bodies and number of generalized velocities


sherm1

Copilot review comments addressed.

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

sherm1 · 2026-06-10T23:56:12Z

+  /// (Internal use only for now) Controls whether welded-together RigidBody
+  /// elements are to be combined into a single composite mobilized body in
+  /// the generated model. If so, the Weld joints will not appear in the
+  /// post-Finalize() model and there will be fewer bodies and joints in the


sherm1 · 2026-06-10T23:56:13Z

-  /// Number of bodies and number of generalized velocities overload. This
-  /// constructor is useful for constructing the MultibodyForces structure
-  /// before a MultibodyPlant has been constructed.
+  /// (Advanced) Number of bodies and number of generalized velocities


sherm1 · 2026-06-10T23:56:13Z

+void MultibodyTree<T>::SetCombineWeldedBodies(
+    bool combine, std::optional<ModelInstanceIndex> model_instance) {
+  LinkJointGraph& graph = mutable_graph();


sherm1 · 2026-06-10T23:56:14Z

+        frame_body_poses->Set_p_BoLcm_B(link.ordinal(), p_BoLcm_B);
+      } else {
+        // X_BL is identity.
+        frame_body_poses->AddToM_BBo_B(mobod.index(), M_LLo_L);
+        frame_body_poses->Set_p_BoLcm_B(link.ordinal(), p_LoLcm_L);


mitiguy

Sherm -- feel free to merge the comments here that make sense. I'll keep going while you are gone.

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

multibody/plant/test/composite_test.cc line 35 at r3 (raw file):

// Tolerance for numerical comparisons.
constexpr double kTolerance = 8 * std::numeric_limits<double>::epsilon();

nit Consider giving an additional bit of slop now to avoid a months later CI failure due to some hardware/compiler difference.

Suggestion:

constexpr double kTolerance = 16 * std::numeric_limits<double>::epsilon();

multibody/plant/test/composite_test.cc line 64 at r3 (raw file):

from Link2's frame.

At joint angle θ, the link origins in World are:

nit More explicit language.

Suggestion:

The positions of the link origins from Wo (World origin), expressed in World are:

multibody/plant/test/composite_test.cc line 72 at r3 (raw file):

Link1 via revolute, Link2 and Link3 each via weld). With
combine_welded_bodies=true we get World plus a single composite mobilized body
that contains all three links. */

nit "Call out" 4 vs 2 mobods. Spacing around = . Change layout to improve readability.

Suggestion:

With combine_welded_bodies = false, four mobilized bodies are created 
(World, Link1 via revolute, Link2 via weld, and Link3 via weld). 
With combine_welded_bodies = true two mobilized bodies are created, 
(World and one composite mobilized body that contains all three links. */

multibody/plant/test/composite_test.cc line 78 at r3 (raw file):

  m.plant->SetCombineWeldedBodies(combine_welded_bodies);

  // Give each link a non-trivial inertia: a 1 kg solid cube, 0.1 m per side.

This comment is misleading. The inertia is "trivial". As is mentioned in a subsequent comment, a solid cube is triaxially symmetric, which means that (like a solid sphere) the moment of inertia about any line that passes through its center of mass is identical.

Suggestion:

  // To faciliate an analytical solution, each link has a trivial inertia,
  // namely a 1 kg solid cube, 0.1 m per side.

multibody/plant/test/composite_test.cc line 87 at r3 (raw file):

  m.link3 = &m.plant->AddRigidBody("Link3", M);

  // Revolute joint: World → Link1, rotating about z.

nit The unicode right-arrow → does not show up well in reviewable. It is too low.
Consider a double right-arrow ⇒ or bold right arrow ➔ or simply "to".

Suggestion:

  // Revolute joint: World to Link1, rotating about z.

multibody/plant/test/composite_test.cc line 108 at r3 (raw file):

  // Sanity check that we got what we asked for. Both models have the bodies
  // (links), joints, and number of states in common, but they should differ in
  // the number of mobilized bodies.

nit Shorten

Suggestion:

  // Sanity check: Both models should have the same number of bodies (links), joints, 
  // and number of states, but they should differ in the number of mobilized bodies.

multibody/plant/test/composite_test.cc line 112 at r3 (raw file):

  EXPECT_EQ(m.plant->num_joints(), 3);
  EXPECT_EQ(m.plant->num_positions(), 1);
  EXPECT_EQ(m.plant->num_velocities(), 1);

nit Improve comments.

Suggestion:

  EXPECT_EQ(m.plant->num_bodies(), 4);  // World + 3 links.
  EXPECT_EQ(m.plant->num_joints(), 3);  // 1 revolute + 2 welds.
  EXPECT_EQ(m.plant->num_positions(), 1);  // 1 revolute angle.
  EXPECT_EQ(m.plant->num_velocities(), 1);  // 1 revolute angular rate.

multibody/plant/test/composite_test.cc line 148 at r3 (raw file):

  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)

nit Improve comments. Sherm -- check to ensure I have p₁, p₂, p₃ properly documented.

Suggestion:

links, each a solid cube of mass m = 1 kg and side length a = 0.1 m. 
The body-frame origins of each of the 3 links are coincident with 
their centers of mass. The positions p₁, p₂, p₃ of each link's origin 
from Mo (the origin of the revolute joint's mobilizer M frame), 
expressed in frame M are (frame M is coincident with Link1's frame):
  Link1: p₁ = (0, 0, 0)   Link1's origin is at the revolute joint.
  Link2: p₂ = (1, 0, 0)   Link2's origin from Mo, expressed in M.
  Link3: p₃ = (1, 1, 0)   Link3's origin from Mo, expressed in M.

multibody/plant/test/composite_test.cc line 162 at r3 (raw file):

  Link2: I₂ = 1*(0.1)²/6 + 1*1²    = 1/600 + 1   (d² = 1)
  Link3: I₃ = 1*(0.1)²/6 + 1*√2²   = 1/600 + 2   (d² = 2)
  Total: M = 3/600 + 3 = 1/200 + 3 = 3.005 kg·m² */

nit Use m and a. Do not plug in numbers until later?

Suggestion:

  Link1: I₁ = m*a²/6 + m*d₁²   (d₁² = 0)
  Link2: I₂ = m*a²/6 + m*d₂²   (d₂² = 1)
  Link3: I₃ = m*a²/6 + m*d₃²   (d₃² = 2)
  Total: I = m*a²/2 + m*(d₁² + d₂² + d₃²) */

multibody/plant/multibody_plant.h line 1810 at r3 (raw file):

  /// Finalize(); post-finalize it returns the setting that _was_ used if
  /// there were any welded-together bodies.
  /// @see SetCombineWeldedBodies(), GetBaseBodyJointType(), Finalize()

nit Shorten and clarify?

Suggestion:

  /// (Internal use only for now) Returns the global or model_instance setting
  /// for whether or not to combine welded RigidBody elements.
  /// @param[in] model_instance (optional). If this argument is missing or
  /// not recognized, returns the global setting. Otherwise returns the 
  /// setting for this specific model_instance. By default, the global 
  /// setting and the setting within a model_instance are false.
  /// @note This function can be called pre-Finalize() or post-Finalize().
  /// @see SetCombineWeldedBodies(), GetBaseBodyJointType(), Finalize()

multibody/plant/multibody_plant.h line 1770 at r2 (raw file):

  /// elements are to be combined into a single composite mobilized body in
  /// the generated model. If so, the Weld joints will not appear in the
  /// post-Finalize() model and there will be fewer bodies and joints in the

nit I like that copilot flagged this.
Maybe what is below?

Suggestion:

  /// post-Finalize() model and the number of mobolized bodies (mobods) and
  /// associated mobilizers will decrease by the number of weld joints,
  /// and thus the number of mobods (and mobilizers) in the generated model 
  /// can be smaller than the number of bodies in the user's specification.

multibody/plant/multibody_plant.h line 1778 at r2 (raw file):

  /// You can set this globally or on a per-model instance basis.
  ///
  /// The default for Drake is _not_ to combine welded RigidBody elements.

nit I think this would be clearer if you provide @param for each of the possible arguments.

Suggestion:

  /// @param[in] combine If the 2nd argument is missing, sets the global
  /// boolean that dictates whether to combine all welded RigidBody 
  /// elements which have not otherwise been set by a per-model instance.
  /// By default, combine is false (welded RigidBody elements are not combined).  
  /// @param[in] model_instance (optional) specifies those welded bodies which
  /// can override the aforementioned global boolean with its own true/false.

sherm1

All comments addressed and updated in my branch sherm1/calc_composite_mass_properties. Here's how you can start your own branch from this one:

# One-time set up
git remote add sherm1 https://github.com/sherm1/drake.git
git fetch sherm1

# Create your branch based on this one and switch to it
git checkout -b your_new_branch_name sherm1/calc_composite_mass_properties

@sherm1 made 13 comments and resolved 11 discussions.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on mitiguy).

multibody/plant/multibody_plant.h line 1770 at r2 (raw file):