Add option to build composites and compute kinematics

Working on body->link terminology in multibodytree

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

Author: sherm1

bindings/generated_docstrings/multibody_plant.h

@@ -186,32 +186,6 @@ This method must be called pre-Finalize.
 Raises:
     RuntimeError if ``plant`` is finalized or if time_step is changed.)""";
       } ApplyMultibodyPlantConfig;
-      // Symbol: drake::multibody::BaseBodyJointType
-      struct /* BaseBodyJointType */ {
-        // Source: drake/multibody/plant/multibody_plant.h
-        const char* doc =
-R"""(The kind of joint to be used to connect base bodies to world at
-Finalize(). See mbp_working_with_free_bodies "Working with free
-bodies" for definitions and discussion.
-
-See also:
-    SetBaseBodyJointType() for details.)""";
-        // Symbol: drake::multibody::BaseBodyJointType::kQuaternionFloatingJoint
-        struct /* kQuaternionFloatingJoint */ {
-          // Source: drake/multibody/plant/multibody_plant.h
-          const char* doc = R"""(6 dofs, unrestricted orientation.)""";
-        } kQuaternionFloatingJoint;
-        // Symbol: drake::multibody::BaseBodyJointType::kRpyFloatingJoint
-        struct /* kRpyFloatingJoint */ {
-          // Source: drake/multibody/plant/multibody_plant.h
-          const char* doc = R"""(6 dofs using 3 angles; has singularity.)""";
-        } kRpyFloatingJoint;
-        // Symbol: drake::multibody::BaseBodyJointType::kWeldJoint
-        struct /* kWeldJoint */ {
-          // Source: drake/multibody/plant/multibody_plant.h
-          const char* doc = R"""(0 dofs, fixed to World.)""";
-        } kWeldJoint;
-      } BaseBodyJointType;
       // Symbol: drake::multibody::CalcContactFrictionFromSurfaceProperties
       struct /* CalcContactFrictionFromSurfaceProperties */ {
         // Source: drake/multibody/plant/coulomb_friction.h
@@ -4968,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())""";
+    SetBaseBodyJointType(), GetCombineWeldedBodies(), Finalize())""";
         } GetBaseBodyJointType;
         // Symbol: drake::multibody::MultibodyPlant::GetBodiesKinematicallyAffectedBy
         struct /* GetBodiesKinematicallyAffectedBy */ {
@@ -5125,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
@@ -6625,8 +6616,33 @@ Parameter ``model_instance``:
     is to be applied.
 
 Raises:
-    RuntimeError if called after Finalize().)""";
+    RuntimeError if called after Finalize().
+
+See also:
+    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

@@ -589,6 +589,32 @@ representation.)""";
           const char* doc = R"""()""";
         } type_name;
       } BallRpyJoint;
+      // Symbol: drake::multibody::BaseBodyJointType
+      struct /* BaseBodyJointType */ {
+        // Source: drake/multibody/tree/multibody_tree.h
+        const char* doc =
+R"""(The kind of joint to be used to connect base bodies to world at
+Finalize(). See mbp_working_with_free_bodies "Working with free
+bodies" for definitions and discussion.
+
+See also:
+    SetBaseBodyJointType() for details.)""";
+        // Symbol: drake::multibody::BaseBodyJointType::kQuaternionFloatingJoint
+        struct /* kQuaternionFloatingJoint */ {
+          // Source: drake/multibody/tree/multibody_tree.h
+          const char* doc = R"""(6 dofs, unrestricted orientation.)""";
+        } kQuaternionFloatingJoint;
+        // Symbol: drake::multibody::BaseBodyJointType::kRpyFloatingJoint
+        struct /* kRpyFloatingJoint */ {
+          // Source: drake/multibody/tree/multibody_tree.h
+          const char* doc = R"""(6 dofs using 3 angles; has singularity.)""";
+        } kRpyFloatingJoint;
+        // Symbol: drake::multibody::BaseBodyJointType::kWeldJoint
+        struct /* kWeldJoint */ {
+          // Source: drake/multibody/tree/multibody_tree.h
+          const char* doc = R"""(0 dofs, fixed to World.)""";
+        } kWeldJoint;
+      } BaseBodyJointType;
       // Symbol: drake::multibody::BodyIndex
       struct /* BodyIndex */ {
         // Source: drake/multibody/tree/multibody_tree_indexes.h
@@ -2778,7 +2804,7 @@ extract X_BF for this Frame from it.
     since the last parameter change; we can't check here.
 
 Returns ``X_BF``:
-    pose of this frame in its body's frame)""";
+    pose of this frame in its Mobod's frame)""";
         } get_X_BF;
         // Symbol: drake::multibody::Frame::get_X_FB
         struct /* get_X_FB */ {
@@ -2792,15 +2818,22 @@ extract X_FB (=X_BF⁻¹) for this Frame from it.
     since the last parameter change; we can't check here.
 
 Returns ``X_FB``:
-    inverse of this frame's pose in its body's frame)""";
+    inverse of this frame's pose in its Mobod's frame)""";
         } get_X_FB;
-        // Symbol: drake::multibody::Frame::get_body_pose_index_in_cache
-        struct /* get_body_pose_index_in_cache */ {
+        // Symbol: drake::multibody::Frame::get_X_LF
+        struct /* get_X_LF */ {
           // Source: drake/multibody/tree/frame.h
           const char* doc =
-R"""((Internal use only) Retrieve this Frame's body pose index in the
-cache.)""";
-        } get_body_pose_index_in_cache;
+R"""((Internal use only) Given an already up-to-date frame body pose cache,
+extract X_LF for this Frame from it.
+
+Note:
+    Be sure you have called MultibodyTreeSystem∷EvalFrameBodyPoses()
+    since the last parameter change; we can't check here.
+
+Returns ``X_LF``:
+    pose of this frame in its Link's frame)""";
+        } get_X_LF;
         // Symbol: drake::multibody::Frame::index
         struct /* index */ {
           // Source: drake/multibody/tree/frame.h
@@ -2850,26 +2883,6 @@ name will be empty (the scope name and the element name).
     RuntimeError if this element is not associated with a
     MultibodyPlant.)""";
         } scoped_name;
-        // Symbol: drake::multibody::Frame::set_body_pose_index_in_cache
-        struct /* set_body_pose_index_in_cache */ {
-          // Source: drake/multibody/tree/frame.h
-          const char* doc =
-R"""((Internal use only) A Frame's pose-in-parent X_PF can be
-parameterized, the parent's pose may also be parameterized, and so on.
-Thus the calculation of this frame's pose in its body (X_BF) can be
-expensive. There is a cache entry that holds the calculated X_BF,
-evaluated whenever parameters change. This allows us to grab X_BF as a
-const reference rather than having to extract and reformat parameters,
-and compose with parent and ancestor poses at runtime.
-
-At the time parameters are allocated we assign a slot in the body pose
-cache entry to each Frame and record its index using this function.
-(The index for a RigidBodyFrame will refer to an identity transform.)
-Note that the body pose index is not necessarily the same as the Frame
-index because all RigidBodyFrames can share an entry. (Of course if
-you know you are working with a RigidBodyFrame you don't need to ask
-about its body pose!))""";
-        } set_body_pose_index_in_cache;
       } Frame;
       // Symbol: drake::multibody::FrameIndex
       struct /* FrameIndex */ {
@@ -5115,6 +5128,17 @@ body frame B.)""";
           const char* doc = R"""()""";
         } stiffness;
       } LinearSpringDamper;
+      // Symbol: drake::multibody::LinkIndex
+      struct /* LinkIndex */ {
+        // Source: drake/multibody/tree/multibody_tree_indexes.h
+        const char* doc = R"""(This is a synonym for BodyIndex.)""";
+      } LinkIndex;
+      // Symbol: drake::multibody::LinkOrdinal
+      struct /* LinkOrdinal */ {
+        // Source: drake/multibody/tree/multibody_tree_indexes.h
+        const char* doc =
+R"""(Type used to identify links by ordinal within a multibody plant.)""";
+      } LinkOrdinal;
       // Symbol: drake::multibody::ModelInstanceIndex
       struct /* ModelInstanceIndex */ {
         // Source: drake/multibody/tree/multibody_tree_indexes.h
@@ -7676,6 +7700,14 @@ the index into all associated quantities.)""";
 R"""(Gets the ``name`` associated with this rigid body. The name will never
 be empty.)""";
         } name;
+        // Symbol: drake::multibody::RigidBody::ordinal
+        struct /* ordinal */ {
+          // Source: drake/multibody/tree/rigid_body.h
+          const char* doc =
+R"""((Internal use only) Returns this Link's (RigidBody's) unique ordinal.
+Currently identical to the index but will differ when we permit
+removal of Links as we do for Joints.)""";
+        } ordinal;
         // Symbol: drake::multibody::RigidBody::scoped_name
         struct /* scoped_name */ {
           // Source: drake/multibody/tree/rigid_body.h

bindings/pydrake/multibody/plant_py.cc

@@ -1775,17 +1775,6 @@ PYBIND11_MODULE(plant, m) {
         .value("kLagged", Class::kLagged, cls_doc.kLagged.doc);
   }
 
-  {
-    using Class = BaseBodyJointType;
-    constexpr auto& cls_doc = doc.BaseBodyJointType;
-    py::enum_<Class> cls(m, "BaseBodyJointType", cls_doc.doc);
-    cls.value("kQuaternionFloatingJoint", Class::kQuaternionFloatingJoint,
-           cls_doc.kQuaternionFloatingJoint.doc)
-        .value("kRpyFloatingJoint", Class::kRpyFloatingJoint,
-            cls_doc.kRpyFloatingJoint.doc)
-        .value("kWeldJoint", Class::kWeldJoint, cls_doc.kWeldJoint.doc);
-  }
-
   {
     using Class = MultibodyPlantConfig;
     constexpr auto& cls_doc = doc.MultibodyPlantConfig;

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/benchmarks/kuka_iiwa_robot/drake_kuka_iiwa_robot.h

@@ -85,7 +85,7 @@ class DrakeKukaIIwaRobot {
         MakeKukaIiwaModel<T>(false /* finalized model */,
                              gravity /* acceleration of gravity */));
 
-    linkN_ = &tree().world_body();
+    linkN_ = &tree().world_link();
 
     // Get this robot's seven links.
     linkA_ = &tree().GetRigidBodyByName("iiwa_link_1");
@@ -111,7 +111,7 @@ class DrakeKukaIIwaRobot {
 
   /// This method gets the number of rigid bodies in this robot.
   /// @returns the number of rigid bodies in this robot.
-  int get_number_of_rigid_bodies() const { return tree().num_bodies(); }
+  int get_number_of_rigid_bodies() const { return tree().num_links(); }
 
   /// This method calculates kinematic properties of the end-effector (herein
   /// denoted as rigid body G) of a 7-DOF KUKA LBR iiwa robot (14 kg payload).
@@ -145,7 +145,7 @@ class DrakeKukaIIwaRobot {
     const SpatialVelocity<T>& V_NG_N = vc.get_V_WB(linkG_->mobod_index());
 
     // Retrieve end-effector spatial acceleration from acceleration cache.
-    std::vector<SpatialAcceleration<T>> A_WB(tree().num_bodies());
+    std::vector<SpatialAcceleration<T>> A_WB(tree().num_links());
     // TODO(eric.cousineau): For this model, the end effector's BodyIndex
     // matches its MobodIndex, thus we're not really checking the difference
     // between MultibodyPlant and MultibodyTree's ordering.

multibody/benchmarks/kuka_iiwa_robot/make_kuka_iiwa_model.cc

@@ -91,7 +91,7 @@ KukaIiwaModelBuilder<T>::Build() const {
   // angles and a position vector. Alternately, frame An is regarded as
   // coincident with linkA.
   const Joint<T>* joint{nullptr};
-  const RigidBody<T>& linkN = model->world_body();
+  const RigidBody<T>& linkN = model->world_link();
   joint = &AddRevoluteJointFromSpaceXYZAnglesAndXYZ(
       "iiwa_joint_1", linkN, joint_1_rpy_, joint_1_xyz_, linkA,
       Eigen::Vector3d::UnitZ(), model.get());

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 = [