Compute composite mass properties (#24654)

Co-authored-by: Michael Sherman <sherm1@gmail.com>

Author: mitiguy

bindings/generated_docstrings/multibody_plant.h

@@ -3752,7 +3752,7 @@ method computes:
 where ``M(q)`` is the model's mass matrix (including rigid body mass
 properties and reflected_inertia "reflected inertias"), ``C(q, v)v``
 is the bias term for Coriolis and gyroscopic effects and ``tau_app``
-consists of a vector applied generalized forces. The last term is a
+consists of a vector of applied generalized forces. The last term is a
 summation over all bodies in the model where ``Fapp_Bo_W`` is an
 applied spatial force on body B at ``Bo`` which gets projected into
 the space of generalized forces with the transpose of ``Jv_V_WB(q)``
@@ -5011,7 +5011,7 @@ be used by Finalize(); post-finalize it returns the joint type that
 *was* used if there were any base bodies in need of a joint.
 
 See also:
-    SetBaseBodyJointType(), Finalize())""";
+    SetBaseBodyJointType(), GetCombineWeldedBodies(), Finalize())""";
         } GetBaseBodyJointType;
         // Symbol: drake::multibody::MultibodyPlant::GetBodiesKinematicallyAffectedBy
         struct /* GetBodiesKinematicallyAffectedBy */ {
@@ -5168,6 +5168,25 @@ geometries for ``body`` previously registered with a SceneGraph.
 See also:
     RegisterCollisionGeometry(), Finalize())""";
         } GetCollisionGeometriesForBody;
+        // Symbol: drake::multibody::MultibodyPlant::GetCombineWeldedBodies
+        struct /* GetCombineWeldedBodies */ {
+          // Source: drake/multibody/plant/multibody_plant.h
+          const char* doc =
+R"""((Internal use only for now) Returns the global or a model_instance
+setting for whether or not to combine welded RigidBody (Link)
+elements.
+
+Note:
+    This function can be called pre-Finalize() or post-Finalize().
+
+Parameter ``model_instance``:
+    (optional). If this argument is missing or not recognized, returns
+    the global setting. Otherwise returns the setting for this
+    specific model_instance.
+
+See also:
+    SetCombineWeldedBodies(), GetBaseBodyJointType(), Finalize())""";
+        } GetCombineWeldedBodies;
         // Symbol: drake::multibody::MultibodyPlant::GetConstraintActiveStatus
         struct /* GetConstraintActiveStatus */ {
           // Source: drake/multibody/plant/multibody_plant.h
@@ -6641,8 +6660,45 @@ Parameter ``model_instance``:
     RuntimeError if called after Finalize().
 
 See also:
-    GetBaseBodyJointType(), Finalize())""";
+    GetBaseBodyJointType(), SetCombineWeldedBodies(), Finalize())""";
         } SetBaseBodyJointType;
+        // Symbol: drake::multibody::MultibodyPlant::SetCombineWeldedBodies
+        struct /* SetCombineWeldedBodies */ {
+          // Source: drake/multibody/plant/multibody_plant.h
+          const char* doc =
+R"""((Internal use only for now) Controls whether welded-together RigidBody
+(Link) elements are to be combined into a single composite mobilized
+body in the generated model. If so, those Weld joints will not be
+modeled (i.e. will have no corresponding mobilizer) in the
+post-Finalize() model and there will be fewer mobilized bodies and
+modeled joints in the generated model than in the user's specification
+of links and joints. Results for the original RigidBody (Link)
+elements can still be obtained by name or BodyIndex, but no results
+(in particular, no reaction forces) are available for the unmodeled
+Weld joints.
+
+You can set this flag globally or on a per-model instance basis. If
+there is a setting for a joint's model instance then that setting is
+used; otherwise, the global setting is used.
+
+The default global setting for Drake is *not* to combine welded
+RigidBody elements.
+
+Parameter ``combine``:
+    Whether to combine welded-together bodies. This only affects a
+    particular model instance if the ``model_instance`` argument is
+    also provided, otherwise it sets the global value.
+
+Parameter ``model_instance``:
+    (optional) if present, specifies a particular model instance to
+    which the ``combine`` argument applies.
+
+Raises:
+    RuntimeError if called after Finalize().
+
+See also:
+    GetCombineWeldedBodies(), SetBaseBodyJointType(), Finalize())""";
+        } SetCombineWeldedBodies;
         // Symbol: drake::multibody::MultibodyPlant::SetConstraintActiveStatus
         struct /* SetConstraintActiveStatus */ {
           // Source: drake/multibody/plant/multibody_plant.h

bindings/generated_docstrings/multibody_tree.h

@@ -1956,14 +1956,14 @@ R"""(Returns the owning MultibodyPlant LeafSystem.
         // Source: drake/multibody/tree/force_element.h
         const char* doc =
 R"""(A ForceElement allows modeling state and time dependent forces in a
-MultibodyTree model. Examples of such forces are springs, dampers,
+MultibodyPlant model. Examples of such forces are springs, dampers,
 drag and gravity. Forces that depend on accelerations such as virtual
 mass cannot be modeled with a ForceElement. This abstract class
 provides an API that all force elements subclasses must implement in
 order to be fully defined. These are:
 
 - CalcAndAddForceContribution(): computes the force contribution of a force
-  element in a MultibodyTree model.
+  element in a MultibodyPlant model.
 - CalcPotentialEnergy(): computes a force element potential energy
   contribution.
 - CalcConservativePower(): computes the power generated by conservative
@@ -1980,7 +1980,7 @@ model, different force elements may write into the array of spatial
 forces ``F_B_W`` or the array of generalized forces ``tau``.
 
 Parameter ``context``:
-    The context containing the state of the MultibodyTree model.
+    The context containing the state of the MultibodyPlant model.
 
 Parameter ``pc``:
     A position kinematics cache object already updated to be in sync
@@ -1995,7 +1995,7 @@ Parameter ``forces``:
     output ``this`` force element adds its contribution into
     ``forces``. This method will abort if the ``forces`` pointer is
     null or if the forces object is not compatible with ``this``
-    MultibodyTree, see MultibodyForces∷CheckInvariants().
+    MultibodyPlant, see MultibodyForces∷CheckInvariants().
 
 Precondition:
     The position kinematics ``pc`` must have been previously updated
@@ -2076,6 +2076,13 @@ parameters of this method. It assumes ``forces`` to be a valid pointer
 to a MultibodyForces object compatible with the MultibodyTree model
 owning ``this`` force element.
 
+Note:
+    Although spatial forces are applied to *links*, it is the
+    responsibility of the implementation to shift those forces to the
+    mobilized bodies that the links follow. When we are combining
+    welded-together links into composites, there can be multiple links
+    on the same mobilized body.
+
 Precondition:
     The position kinematics ``pc`` must have been previously updated
     with a call to CalcPositionKinematicsCache().
@@ -4596,16 +4603,16 @@ class documentation states that a Space-fixed (extrinsic) X-Y-Z
 rotation with roll-pitch-yaw angles [q₀ q₁ q₂] is equivalent to a
 Body-fixed (intrinsic) Z-Y-X rotation by yaw-pitch-roll angles [q₂ q₁
 q₀]. In the context of "gimbal torques", the Body-fixed Z-Y-X rotation
-sequence with angles [q₂ q₁ q₀] is physical meaningful as it produces
-torques associated with successive frames in a gimbal as τ₂ 𝐀𝐳, τ₁ 𝐏𝐲,
-τ₀ 𝐂𝐱, where each of 𝐀𝐳, 𝐏𝐲, 𝐂𝐱 are unit vectors associated with a
-frame in the yaw-pitch-roll rotation sequence and 𝐏𝐲 is a unit vector
-of the "pitch" intermediate frame. As described earlier, torque 𝐭 is
-the moment of the bushing forces on frame C about Cp. Scalars tx, ty,
-tz are defined so 𝐭 can be expressed ``𝐭 = [tx ty tz]ᴀ = tx 𝐀𝐱 + ty 𝐀𝐲
-+ tz 𝐀𝐳``. As shown in code documentation, the relationship of [tx ty
-tz] to [τ₀ τ₁ τ₂] was found by equating 𝐭's power to τ's power as 𝐭 ⋅
-w_AC = τ ⋅ q̇.
+sequence with angles [q₂ q₁ q₀] is physically meaningful as it
+produces torques associated with successive frames in a gimbal as τ₂
+𝐀𝐳, τ₁ 𝐏𝐲, τ₀ 𝐂𝐱, where each of 𝐀𝐳, 𝐏𝐲, 𝐂𝐱 are unit vectors associated
+with a frame in the yaw-pitch-roll rotation sequence and 𝐏𝐲 is a unit
+vector of the "pitch" intermediate frame. As described earlier, torque
+𝐭 is the moment of the bushing forces on frame C about Cp. Scalars tx,
+ty, tz are defined so 𝐭 can be expressed ``𝐭 = [tx ty tz]ᴀ = tx 𝐀𝐱 +
+ty 𝐀𝐲 + tz 𝐀𝐳``. As shown in code documentation, the relationship of
+[tx ty tz] to [τ₀ τ₁ τ₂] was found by equating 𝐭's power to τ's power
+as 𝐭 ⋅ w_AC = τ ⋅ q̇.
 
 
 .. raw:: html
@@ -5438,11 +5445,11 @@ that is added during Finalize() should set this flag to ``True``.)""";
       struct /* MultibodyForces */ {
         // Source: drake/multibody/tree/multibody_forces.h
         const char* doc =
-R"""(A class to hold a set of forces applied to a MultibodyTree system.
-Forces can include generalized forces as well as body spatial forces.
-MultibodyPlant∷CalcGeneralizedForces() can be used to compute the
-*total* generalized force, combining generalized_forces() and
-body_forces().)""";
+R"""(Holds a set of forces applied to a MultibodyTree system. Forces can
+include generalized forces as well as mobilized body (mobod) spatial
+forces. MultibodyPlant∷CalcGeneralizedForces() can be used to compute
+the *total* generalized force, combining generalized_forces() with the
+generalized force equivalent of the body_forces().)""";
         // Symbol: drake::multibody::MultibodyForces::AddInForces
         struct /* AddInForces */ {
           // Source: drake/multibody/tree/multibody_forces.h
@@ -5475,9 +5482,13 @@ abort.)""";
           const char* doc_1args_model = R"""((Advanced) Tree overload.)""";
           // Source: drake/multibody/tree/multibody_forces.h
           const char* doc_2args_nb_nv =
-R"""(Number of bodies and number of generalized velocities overload. This
-constructor is useful for constructing the MultibodyForces structure
-before a MultibodyPlant has been constructed.)""";
+R"""((Advanced) Constructs a force object with a given number of mobilized
+bodies and corresponding mobilities (number of generalized
+velocities). This constructor may be useful for constructing the
+MultibodyForces structure before a MultibodyPlant has been
+constructed. However, this must be used cautiously since the number of
+mobilized bodies and their mobilities are not known until after
+Finalize().)""";
         } ctor;
         // Symbol: drake::multibody::MultibodyForces::SetZero
         struct /* SetZero */ {

multibody/plant/BUILD.bazel

@@ -458,6 +458,15 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "composite_test",
+    deps = [
+        ":plant",
+        "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:expect_throws_message",
+    ],
+)
+
 drake_cc_googletest(
     name = "compliant_contact_manager_scalar_conversion_test",
     deps = [

multibody/plant/multibody_plant.cc

@@ -1422,12 +1422,24 @@ void MultibodyPlant<T>::SetBaseBodyJointType(
   mutable_tree().SetBaseBodyJointType(joint_type, model_instance);
 }
 
+template <typename T>
+void MultibodyPlant<T>::SetCombineWeldedBodies(
+    bool combine, std::optional<ModelInstanceIndex> model_instance) {
+  mutable_tree().SetCombineWeldedBodies(combine, model_instance);
+}
+
 template <typename T>
 BaseBodyJointType MultibodyPlant<T>::GetBaseBodyJointType(
     std::optional<ModelInstanceIndex> model_instance) const {
   return internal_tree().GetBaseBodyJointType(model_instance);
 }
 
+template <typename T>
+bool MultibodyPlant<T>::GetCombineWeldedBodies(
+    std::optional<ModelInstanceIndex> model_instance) const {
+  return internal_tree().GetCombineWeldedBodies(model_instance);
+}
+
 template <typename T>
 void MultibodyPlant<T>::Finalize() {
   // After finalizing the base class, tree is read-only.

multibody/plant/multibody_plant.h

@@ -1759,11 +1759,39 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem<T> {
   /// @param[in] model_instance (optional) the index of the model instance to
   ///   which `joint_type` is to be applied.
   /// @throws std::exception if called after Finalize().
-  /// @see GetBaseBodyJointType(), Finalize()
+  /// @see GetBaseBodyJointType(), SetCombineWeldedBodies(), Finalize()
   void SetBaseBodyJointType(
       BaseBodyJointType joint_type,
       std::optional<ModelInstanceIndex> model_instance = {});
 
+  /// (Internal use only for now) Controls whether welded-together RigidBody
+  /// (Link) elements are to be combined into a single composite mobilized
+  /// body in the generated model. If so, those Weld joints will not be modeled
+  /// (i.e. will have no corresponding mobilizer) in the post-Finalize()
+  /// model and there will be fewer mobilized bodies and modeled joints in the
+  /// generated model than in the user's specification of links and joints.
+  /// Results for the original RigidBody (Link) elements can still be obtained
+  /// by name or BodyIndex, but no results (in particular, no reaction forces)
+  /// are available for the unmodeled Weld joints.
+  ///
+  /// You can set this flag globally or on a per-model instance basis. If
+  /// there is a setting for a joint's model instance then that setting is
+  /// used; otherwise, the global setting is used.
+  ///
+  /// The default global setting for Drake is _not_ to combine welded RigidBody
+  /// elements.
+  ///
+  /// @param[in] combine Whether to combine welded-together bodies. This only
+  ///   affects a particular model instance if the `model_instance` argument is
+  ///   also provided, otherwise it sets the global value.
+  /// @param[in] model_instance (optional) if present, specifies a particular
+  ///   model instance to which the `combine` argument applies.
+  ///
+  /// @throws std::exception if called after Finalize().
+  /// @see GetCombineWeldedBodies(), SetBaseBodyJointType(), Finalize()
+  void SetCombineWeldedBodies(
+      bool combine, std::optional<ModelInstanceIndex> model_instance = {});
+
   /// Returns the currently-set choice for base body joint type, either for
   /// the global setting or for a specific model instance if provided.
   /// If a model instance is provided for which no explicit choice has been
@@ -1772,10 +1800,22 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem<T> {
   /// This can be called any time -- pre-finalize it returns the joint type
   /// that will be used by Finalize(); post-finalize it returns the joint type
   /// that _was_ used if there were any base bodies in need of a joint.
-  /// @see SetBaseBodyJointType(), Finalize()
+  /// @see SetBaseBodyJointType(), GetCombineWeldedBodies(), Finalize()
   BaseBodyJointType GetBaseBodyJointType(
       std::optional<ModelInstanceIndex> model_instance = {}) const;
 
+  /// (Internal use only for now) Returns the global or a model_instance setting
+  /// for whether or not to combine welded RigidBody (Link) elements.
+  ///
+  /// @note This function can be called pre-Finalize() or post-Finalize().
+  ///
+  /// @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.
+  /// @see SetCombineWeldedBodies(), GetBaseBodyJointType(), Finalize()
+  bool GetCombineWeldedBodies(
+      std::optional<ModelInstanceIndex> model_instance = {}) const;
+
   /// This method must be called after all elements in the model (joints,
   /// bodies, force elements, constraints, etc.) are added and before any
   /// computations are performed.
@@ -4142,12 +4182,13 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem<T> {
   /// where `M(q)` is the model's mass matrix (including rigid body mass
   /// properties and @ref reflected_inertia "reflected inertias"), `C(q, v)v` is
   /// the bias term for Coriolis and gyroscopic effects and `tau_app` consists
-  /// of a vector applied generalized forces. The last term is a summation over
-  /// all bodies in the model where `Fapp_Bo_W` is an applied spatial force on
-  /// body B at `Bo` which gets projected into the space of generalized forces
-  /// with the transpose of `Jv_V_WB(q)` (where `Jv_V_WB` is B's spatial
-  /// velocity Jacobian in W with respect to generalized velocities v).
-  /// Note: B's spatial velocity in W can be written as `V_WB = Jv_V_WB * v`.
+  /// of a vector of applied generalized forces. The last term is a summation
+  /// over all bodies in the model where `Fapp_Bo_W` is an applied spatial
+  /// force on body B at `Bo` which gets projected into the space of
+  /// generalized forces with the transpose of `Jv_V_WB(q)` (where `Jv_V_WB`
+  /// is B's spatial velocity Jacobian in W with respect to generalized
+  /// velocities v). Note: B's spatial velocity in W can be written as `V_WB
+  /// = Jv_V_WB * v`.
   ///
   /// This method does not compute explicit expressions for the mass matrix nor
   /// for the bias term, which would be of at least `O(n²)` complexity, but it