Velocity kinematics for composites working; needs tests.

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

@@ -1257,7 +1257,7 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem<T> {
   const systems::OutputPort<T>& get_generalized_contact_forces_output_port(
       ModelInstanceIndex model_instance) const;
 
-  // BLAH(sherm1) Modify the next comment to explain that unmodeled joints will
+  // TODO(sherm1) Modify the next comment to explain that unmodeled joints will
   //  have NaN reaction force entries here. (Joints can be unmodeled because
   //  they were removed or because they are internal to a composite body.)
 

multibody/plant/test/multibody_plant_forward_dynamics_test.cc

@@ -113,11 +113,11 @@ class KukaIiwaModelForwardDynamicsTests : public test::KukaIiwaModelTests {
     // matrix.
     const int nv = plant_->num_velocities();
     MatrixX<double> M(nv, nv);
-    plant_->CalcMassMatrixViaInverseDynamics(*context_, &M);
+    plant_->CalcMassMatrix(*context_, &M);
     const double kappa = 1.0 / M.llt().rcond();
 
-    // Compare expected results against actual vdot.
-    const double kRelativeTolerance = kappa * kEpsilon;
+    // Compare expected results against actual vdot (add a little slop).
+    const double kRelativeTolerance = 2 * kappa * kEpsilon;
     EXPECT_TRUE(CompareMatrices(vdot, vdot_expected, kRelativeTolerance,
                                 MatrixCompareType::relative));
   }
@@ -189,6 +189,14 @@ GTEST_TEST(MultibodyPlantForwardDynamics, AtlasRobot) {
       .FixValue(context.get(), VectorX<double>::Zero(num_actuators));
   auto derivatives = plant.AllocateTimeDerivatives();
   {
+    // Make sure to get global and parameter-change heap allocations done
+    // before we check CalcTimeDerivatives() heap use.
+
+    // Initialization of Highway may use heap on the first call.
+    const math::RotationMatrix<double> R_AB, R_BC;  // Both identity.
+    EXPECT_TRUE((R_AB * R_BC).IsExactlyIdentity());
+    plant.EvalFrameBodyPoses(*context);  // Updated when parameters change.
+
     // CalcTimeDerivatives should not be allocating, but for now we have a few
     // remaining fixes before it's down to zero:
     //  2 temps in MbTS::CalcArticulatedBodyForceCache (F_B_W_, tau_).

multibody/rational/rational_forward_kinematics.cc

@@ -245,12 +245,12 @@ RationalForwardKinematics::CalcChildBodyPoseAsMultilinearPolynomial(
 
   internal::MobodIndex mobilizer_index;
   bool is_order_reversed{};
-  if (parent_mobod.inboard().is_valid() &&
-      parent_mobod.inboard() == child_mobod_index) {
+  if (parent_mobod.inboard_mobod().is_valid() &&
+      parent_mobod.inboard_mobod() == child_mobod_index) {
     is_order_reversed = true;
     mobilizer_index = parent_mobod_index;
-  } else if (child_mobod.inboard().is_valid() &&
-             child_mobod.inboard() == parent_mobod_index) {
+  } else if (child_mobod.inboard_mobod().is_valid() &&
+             child_mobod.inboard_mobod() == parent_mobod_index) {
     is_order_reversed = false;
     mobilizer_index = child_mobod_index;
   } else {

multibody/rational/rational_forward_kinematics_internal.cc

@@ -46,16 +46,17 @@ std::vector<BodyIndex> FindPath(const MultibodyPlant<double>& plant,
         forest.mobods(current_link.mobod_index());
     if (current != world_index()) {
       const SpanningForest::Mobod& parent_mobod =
-          forest.mobods(current_mobod.inboard());
+          forest.mobods(current_mobod.inboard_mobod());
       const BodyIndex parent =
-          tree.forest().links(parent_mobod.link_ordinal()).index();
+          tree.forest().links(parent_mobod.active_link_ordinal()).index();
       visit_edge(current, parent);
     }
-    for (const MobodIndex& child_mobod_index : current_mobod.outboards()) {
+    for (const MobodIndex& child_mobod_index :
+         current_mobod.outboard_mobods()) {
       const SpanningForest::Mobod& child_mobod =
           forest.mobods(child_mobod_index);
       const BodyIndex child =
-          tree.forest().links(child_mobod.link_ordinal()).index();
+          tree.forest().links(child_mobod.active_link_ordinal()).index();
       visit_edge(current, child);
     }
   }
@@ -86,7 +87,8 @@ std::vector<MobodIndex> FindMobilizersOnPath(
     const LinkJointGraph::Link& link_i = forest.link_by_index(path[i]);
     const LinkJointGraph::Link& link_ip1 = forest.link_by_index(path[i + 1]);
     const SpanningForest::Mobod& mobod_i = forest.mobods(link_i.mobod_index());
-    if (!mobod_i.is_world() && mobod_i.inboard() == link_ip1.mobod_index()) {
+    if (!mobod_i.is_world() &&
+        mobod_i.inboard_mobod() == link_ip1.mobod_index()) {
       // path[i] is the child of path[i+1] in MultibodyTreeTopology, they are
       // connected by path[i]'s inboard mobilizer.
       mobilizers_on_path.push_back(link_i.mobod_index());

multibody/tree/body_node.h

@@ -260,7 +260,8 @@ class BodyNode : public MultibodyElement<T> {
   // double_pendulum_test.cc, and by any other unit tests making use of
   // MultibodyTree::CalcVelocityKinematicsCache().
   virtual void CalcVelocityKinematicsCache_BaseToTip(
-      const T* positions, const PositionKinematicsCache<T>& pc,
+      const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
+      const PositionKinematicsCache<T>& pc,
       const std::vector<Vector6<T>>& H_PB_W_cache, const T* velocities,
       VelocityKinematicsCache<T>* vc) const = 0;