From c751eb0dda7ca1f6c07473188f94634515a433d9 Mon Sep 17 00:00:00 2001 From: castor639 <316019788+castor639@users.noreply.github.com> Date: Mon, 31 Aug 2026 20:10:03 +0000 Subject: [PATCH 1/2] [multibody] Throw if CalcSpatialInertia frame is not in the plant Validate that frame_F belongs to this MultibodyTree before computing spatial inertia, so an unowned FixedOffsetFrame throws instead of segfaulting. Fixes #22636. --- multibody/plant/multibody_plant.h | 1 + .../plant/test/multibody_plant_momentum_energy_test.cc | 9 +++++++++ multibody/tree/multibody_tree.cc | 3 +++ 3 files changed, 13 insertions(+) diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h index a173ad91ffcc..1a5fb6892a8f 100644 --- a/multibody/plant/multibody_plant.h +++ b/multibody/plant/multibody_plant.h @@ -4165,6 +4165,7 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// the expressed-in frame for the returned spatial inertia. /// @param[in] body_indexes Array of selected bodies. This method does not /// distinguish between welded bodies, joint-connected bodies, etc. + /// @throws std::exception if `frame_F` does not belong to `this` plant. /// @throws std::exception if body_indexes contains an invalid BodyIndex or /// if there is a repeated BodyIndex. /// @note The mass and inertia of the world_body() does not contribute to the diff --git a/multibody/plant/test/multibody_plant_momentum_energy_test.cc b/multibody/plant/test/multibody_plant_momentum_energy_test.cc index 856735702468..cdebf2888875 100644 --- a/multibody/plant/test/multibody_plant_momentum_energy_test.cc +++ b/multibody/plant/test/multibody_plant_momentum_energy_test.cc @@ -7,6 +7,7 @@ #include "drake/common/test_utilities/eigen_matrix_compare.h" #include "drake/common/test_utilities/expect_throws_message.h" #include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/tree/fixed_offset_frame.h" #include "drake/multibody/tree/multibody_tree_indexes.h" #include "drake/multibody/tree/revolute_joint.h" #include "drake/multibody/tree/rigid_body.h" @@ -335,6 +336,14 @@ TEST_F(TwoDofPlanarPendulumTest, CalcSpatialInertia) { DRAKE_EXPECT_THROWS_MESSAGE( plant_.CalcSpatialInertia(*context_, frame_A, body_indexes), "CalcSpatialInertia\\(\\): contains a repeated BodyIndex.*"); + + // Verify an exception is thrown if frame_F was never added to the plant + // (regression test for #22636). + const FixedOffsetFrame orphan_frame( + "orphan_frame", frame_A, math::RigidTransformd()); + DRAKE_EXPECT_THROWS_MESSAGE( + plant_.CalcSpatialInertia(*context_, orphan_frame, {body_A.index()}), + ".*does not belong to the supplied MultibodyTree.*"); } } // namespace diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc index c4064aff87bb..b177ed4640c9 100644 --- a/multibody/tree/multibody_tree.cc +++ b/multibody/tree/multibody_tree.cc @@ -2634,6 +2634,9 @@ template SpatialInertia MultibodyTree::CalcSpatialInertia( const systems::Context& context, const Frame& frame_F, const std::vector& link_indexes) const { + // Ensure frame_F belongs to this tree; otherwise pose queries can segfault. + frame_F.HasThisParentTreeOrThrow(this); + // Check if there are repeated LinkIndex in link_indexes by converting the // vector to a set (to eliminate duplicates) and see if their sizes differ. const std::set without_duplicate_bodies(link_indexes.begin(), From f6bde33d00255d6c844d3e6d096b73763302af76 Mon Sep 17 00:00:00 2001 From: castor639 <316019788+castor639@users.noreply.github.com> Date: Tue, 1 Sep 2026 22:31:56 +0000 Subject: [PATCH 2/2] [bindings] Regenerate MultibodyPlant docstrings for CalcSpatialInertia --- bindings/generated_docstrings/multibody_plant.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/bindings/generated_docstrings/multibody_plant.h b/bindings/generated_docstrings/multibody_plant.h index bd39e54e858f..4cb65a96b2d8 100644 --- a/bindings/generated_docstrings/multibody_plant.h +++ b/bindings/generated_docstrings/multibody_plant.h @@ -4528,6 +4528,9 @@ Parameter ``body_indexes``: Array of selected bodies. This method does not distinguish between welded bodies, joint-connected bodies, etc. +Raises: + RuntimeError if ``frame_F`` does not belong to ``this`` plant. + Raises: RuntimeError if body_indexes contains an invalid BodyIndex or if there is a repeated BodyIndex.