Add option to build composites and compute kinematics

Modifies FrameBodyPoseCache, position kinematics cache,
and velocity kinematics cache. Loosens two tolerances so that Mac CI passes.

Rebased on body->link terminology change.

Author: sherm1

bindings/pydrake/multibody/test/plant_test.py

@@ -59,7 +59,6 @@
     AddMultibodyPlant,
     AddMultibodyPlantSceneGraph,
     ApplyMultibodyPlantConfig,
-    BaseBodyJointType,
     CalcContactFrictionFromSurfaceProperties,
     ConnectContactResultsToDrakeVisualizer,
     ContactModel,
@@ -81,6 +80,7 @@
 )
 from pydrake.multibody.tree import (
     BallRpyJoint_,
+    BaseBodyJointType,
     Body_,  # dispreferred alias for RigidBody_
     BodyIndex,
     CalcSpatialInertia,

bindings/pydrake/multibody/tree_py.cc

@@ -148,6 +148,17 @@ void DoScalarIndependentDefinitions(py::module m) {
         .value("kV", Enum::kV, enum_doc.kV.doc);
   }
 
+  {
+    using Enum = BaseBodyJointType;
+    constexpr auto& cls_doc = doc.BaseBodyJointType;
+    py::enum_<Enum> cls(m, "BaseBodyJointType", cls_doc.doc);
+    cls.value("kQuaternionFloatingJoint", Enum::kQuaternionFloatingJoint,
+           cls_doc.kQuaternionFloatingJoint.doc)
+        .value("kRpyFloatingJoint", Enum::kRpyFloatingJoint,
+            cls_doc.kRpyFloatingJoint.doc)
+        .value("kWeldJoint", Enum::kWeldJoint, cls_doc.kWeldJoint.doc);
+  }
+
   {
     using Class = ScopedName;
     constexpr auto& cls_doc = doc.ScopedName;

multibody/optimization/test/toppra_test.cc

@@ -210,7 +210,7 @@ TEST_F(IiwaToppraTest, JointTorqueLimit) {
   auto s_path = result.value();
 
   // This tolerance was tuned to work with the given gridpoints.
-  const double tol = 1e-14;
+  const double tol = 1e-13;
   Eigen::MatrixXd M(7, 7);
   Eigen::VectorXd Cv(7);
   Eigen::VectorXd G(7);

multibody/plant/BUILD.bazel

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

multibody/plant/multibody_plant.cc

@@ -1421,12 +1421,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

@@ -1760,11 +1760,28 @@ 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 = {});
 
+  /// 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 generated model than in the user's
+  /// specification. Results for the original RigidBody 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 globally or on a per-model instance basis.
+  ///
+  /// The default for Drake is _not_ to combine welded RigidBody elements.
+  ///
+  /// @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
@@ -1773,10 +1790,23 @@ 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;
 
+  /// Returns the currently-set choice for whether welded-together bodies
+  /// should be combined or modeled separately, either for the global setting
+  /// or for a specific model instance. If a model instance is provided for
+  /// which no explicit choice was made, the global setting is returned. Any
+  /// model instance index is acceptable here; if not recognized then the global
+  /// setting is returned. This can be called any time -- pre-finalize it
+  /// returns the setting that will be used by Finalize(); post-finalize it
+  /// returns the setting that _was_ used if there were any welded-together
+  /// bodies.
+  /// @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.