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/generated_docstrings/multibody_plant.h

@@ -4942,7 +4942,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 */ {
@@ -5099,6 +5099,23 @@ 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"""(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 also:
+    SetCombineWeldedBodies(), GetBaseBodyJointType(), Finalize())""";
+        } GetCombineWeldedBodies;
         // Symbol: drake::multibody::MultibodyPlant::GetConstraintActiveStatus
         struct /* GetConstraintActiveStatus */ {
           // Source: drake/multibody/plant/multibody_plant.h
@@ -6602,8 +6619,30 @@ 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"""(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.
+
+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

@@ -7370,7 +7370,8 @@ R"""(For a floating base RigidBody, unlock its inboard joint.
         struct /* body_frame */ {
           // Source: drake/multibody/tree/rigid_body.h
           const char* doc =
-R"""((Compatibility) A synonym for link_frame().)""";
+R"""(Returns a const reference to the associated RigidBodyFrame
+(LinkFrame). This is a synonym for link_frame().)""";
         } body_frame;
         // Symbol: drake::multibody::RigidBody::default_com
         struct /* default_com */ {
@@ -7531,14 +7532,14 @@ Returns ``alpha_WB_W``:
         struct /* get_angular_velocity_in_world */ {
           // Source: drake/multibody/tree/rigid_body.h
           const char* doc =
-R"""((Advanced) Extract this body's angular velocity in world, expressed in
+R"""((Advanced) Extract this link's angular velocity in world, expressed in
 world.
 
 Parameter ``vc``:
     velocity kinematics cache.
 
-Returns ``w_WB_W``:
-    rigid body B's angular velocity in world W, expressed in W.)""";
+Returns ``w_WL_W``:
+    link L's angular velocity in world W, expressed in W.)""";
         } get_angular_velocity_in_world;
         // Symbol: drake::multibody::RigidBody::get_mass
         struct /* get_mass */ {
@@ -7575,35 +7576,35 @@ body's origin, expressed in world.
 Parameter ``pc``:
     position kinematics cache.
 
-Returns ``p_WoBo_W``:
-    position vector from Wo (world origin) to Bo (this body's origin)
+Returns ``p_WoLo_W``:
+    position vector from Wo (world origin) to Lo (this link's origin)
     expressed in W (world).)""";
         } get_origin_position_in_world;
         // Symbol: drake::multibody::RigidBody::get_origin_velocity_in_world
         struct /* get_origin_velocity_in_world */ {
           // Source: drake/multibody/tree/rigid_body.h
           const char* doc =
-R"""((Advanced) Extract the velocity of this body's origin in world,
+R"""((Advanced) Extract the velocity of this link's origin in world,
 expressed in world.
 
 Parameter ``vc``:
     velocity kinematics cache.
 
-Returns ``v_WBo_W``:
-    velocity of Bo (body origin) in world W, expressed in W.)""";
+Returns ``v_WLo_W``:
+    velocity of Lo (link frame origin) in world W, expressed in W.)""";
         } get_origin_velocity_in_world;
         // Symbol: drake::multibody::RigidBody::get_pose_in_world
         struct /* get_pose_in_world */ {
           // Source: drake/multibody/tree/rigid_body.h
           const char* doc =
-R"""((Advanced) Extract this body's pose in world (from the position
+R"""((Advanced) Extract this link's pose in world (from the position
 kinematics).
 
 Parameter ``pc``:
     position kinematics cache.
 
-Returns ``X_WB``:
-    pose of rigid body B in world frame W.)""";
+Returns ``X_WL``:
+    pose of this Link (%RigidBody) L in world frame W.)""";
         } get_pose_in_world;
         // Symbol: drake::multibody::RigidBody::get_rotation_matrix_in_world
         struct /* get_rotation_matrix_in_world */ {
@@ -7615,8 +7616,9 @@ body's frame.
 Parameter ``pc``:
     position kinematics cache.
 
-Returns ``R_WB``:
-    rotation matrix relating rigid body B in world frame W.)""";
+Returns ``R_WL``:
+    rotation matrix giving the orientation of this Link L in the world
+    frame W.)""";
         } get_rotation_matrix_in_world;
         // Symbol: drake::multibody::RigidBody::get_spatial_acceleration_in_world
         struct /* get_spatial_acceleration_in_world */ {
@@ -7636,15 +7638,15 @@ Returns ``A_WB_W``:
         struct /* get_spatial_velocity_in_world */ {
           // Source: drake/multibody/tree/rigid_body.h
           const char* doc =
-R"""((Advanced) Returns V_WB, this RigidBody B's SpatialVelocity in the
-world frame W.
+R"""((Advanced) Returns V_WL, this Link (%RigidBody) L's SpatialVelocity in
+the world frame W.
 
 Parameter ``vc``:
     velocity kinematics cache.
 
-Returns ``V_WB_W``:
-    this rigid body B's spatial velocity in the world frame W,
-    expressed in W (for point Bo, the body frame's origin).)""";
+Returns ``V_WL_W``:
+    this link L's spatial velocity in the world frame W, expressed in
+    W (for point Lo, the link frame's origin).)""";
         } get_spatial_velocity_in_world;
         // Symbol: drake::multibody::RigidBody::has_quaternion_dofs
         struct /* has_quaternion_dofs */ {
@@ -7682,7 +7684,7 @@ R"""((Deprecated.)
           // Source: drake/multibody/tree/rigid_body.h
           const char* doc =
 R"""((Advanced) Returns ``True`` if this body is a *floating base body*,
-meaning it had no explicit joint to a parent body so is mobilized by
+meaning it had no explicit joint to a parent body and is mobilized by
 an automatically-added (ephemeral) floating (6 dof) joint to World.
 
 Note:
@@ -7713,7 +7715,7 @@ generally Joint∷is_locked() is preferable otherwise.
           // Source: drake/multibody/tree/rigid_body.h
           const char* doc =
 R"""(Returns a const reference to the associated LinkFrame
-(RigidBodyFrame).)""";
+(RigidBodyFrame). This is a synonym for body_frame().)""";
         } link_frame;
         // Symbol: drake::multibody::RigidBody::mobod_index
         struct /* mobod_index */ {

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.