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.

Working on acceleration kinematics.
Working on force elements

Author: sherm1

bindings/generated_docstrings/multibody_plant.h

@@ -3744,7 +3744,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)``
@@ -5003,7 +5003,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 */ {
@@ -5160,6 +5160,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
@@ -6633,8 +6650,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

@@ -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
@@ -5016,9 +5023,9 @@ to choose torque stiffness and damping constants" for more details.)""";
         // Source: drake/multibody/tree/linear_spring_damper.h
         const char* doc =
 R"""(This ForceElement models a spring-damper attached between two points
-on two different bodies (links). Given a point P on a body A and a
-point Q on a body B with positions p_AP and p_BQ, respectively, this
-spring-damper applies equal and opposite forces on bodies A and B
+on two different links (bodies). Given a point P on a link A and a
+point Q on a link B with positions p_AP and p_BQ, respectively, this
+spring-damper applies equal and opposite forces on links A and B
 according to:
 
 
@@ -5040,7 +5047,7 @@ its rate of change, ``r̂ = (p_WQ - p_WP) / ℓ`` is the normalized
 vector from P to Q, ℓ₀ is the free length of the spring and k and c
 are the stiffness and damping of the spring-damper, respectively. This
 ForceElement is meant to model finite free length springs attached
-between two points. In this typical arrangement springs are usually
+between two points. In the typical arrangement springs are usually
 pre-loaded, meaning they apply a non-zero spring force in the static
 configuration of the system. Thus, neither the free length ℓ₀ nor the
 current length ℓ of the spring can ever be zero. The length of the
@@ -5049,7 +5056,7 @@ and therefore this element throws a RuntimeError exception in that
 case. Note that:
 
 - The applied force is always along the line connecting points P and
-Q. - Damping always dissipates energy. - Forces on bodies A and B are
+Q. - Damping always dissipates energy. - Forces on links A and B are
 equal and opposite according to Newton's third law.)""";
         // Symbol: drake::multibody::LinearSpringDamper::CalcConservativePower
         struct /* CalcConservativePower */ {
@@ -5085,11 +5092,11 @@ equal and opposite according to Newton's third law.)""";
         struct /* ctor */ {
           // Source: drake/multibody/tree/linear_spring_damper.h
           const char* doc =
-R"""(Constructor for a spring-damper between a point P on ``bodyA`` and a
-point Q on ``bodyB``. Point P is defined by its position ``p_AP`` as
-measured and expressed in the body frame A and similarly, point Q is
-defined by its position p_BQ as measured and expressed in body frame
-B. The remaining parameters define:
+R"""(Constructor for a spring-damper between a point P on link ``bodyA``
+and a point Q on link ``bodyB``. Point P is defined by its position
+``p_AP`` as measured and expressed in the link frame A and similarly,
+point Q is defined by its position p_BQ as measured and expressed in
+link frame B. The remaining parameters define:
 
 Parameter ``free_length``:
     The free length of the spring ℓ₀, in meters, at which the spring
@@ -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,12 @@ 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) Number of bodies and number of generalized velocities
+overload. 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 */ {
@@ -7040,6 +7050,11 @@ its parent body in the multibody tree by its Mobilizer (also called a
 permissible motion can be added using Constraint objects to remove
 more degrees of freedom.
 
+Note:
+    This object corresponds to a "link" in urdf/sdf terminology. We
+    may combine welded-together links into a composite rigid body
+    internally.
+
 - [Goldstein 2001] H Goldstein, CP Poole, JL Safko, Classical Mechanics
                    (3rd Edition), Addison-Wesley, 2001.)""";
         // Symbol: drake::multibody::RigidBody::AddInForce
@@ -7370,7 +7385,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 +7547,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 +7591,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 +7631,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 +7653,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 */ {
@@ -7672,7 +7689,7 @@ See also:
           // 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:
@@ -7703,16 +7720,16 @@ 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 */ {
           // Source: drake/multibody/tree/rigid_body.h
           const char* doc =
-R"""((Advanced) Returns the index of the mobilized body ("mobod") in the
-computational directed forest structure of the owning MultibodyTree to
-which this RigidBody belongs. This serves as the BodyNode index and
-the index into all associated quantities.)""";
+R"""((Advanced) Returns the index of the mobilized body ("mobod") that is
+followed by this RigidBody (Link). This serves as the BodyNode index
+and the index into all associated quantities. More than one link may
+follow the same mobod.)""";
         } mobod_index;
         // Symbol: drake::multibody::RigidBody::name
         struct /* name */ {
@@ -7745,8 +7762,8 @@ name will be empty (the scope name and the element name).
       struct /* RigidBodyFrame */ {
         // Source: drake/multibody/tree/rigid_body.h
         const char* doc =
-R"""(A RigidBodyFrame is a material Frame that serves as the unique
-reference frame for a RigidBody.
+R"""(A RigidBodyFrame (aka LinkFrame) is a material Frame that serves as
+the unique reference frame for a RigidBody (aka Link).
 
 Each RigidBody B has a unique body frame for which we use the same
 symbol B (with meaning clear from context). We represent a body frame

multibody/benchmarking/BUILD.bazel

@@ -40,6 +40,7 @@ drake_cc_googlebench_binary(
         "//multibody/parsing:parser",
         "//tools/performance:fixture_common",
         "//tools/performance:gflags_main",
+        "@gflags",
     ],
     add_test_rule = True,
 )