[multibody] Support fused mobods

bindings/generated_docstrings/multibody_tree.h

@@ -5023,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:
 
 
@@ -5047,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
@@ -5056,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 */ {
@@ -5092,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
@@ -7051,6 +7051,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
@@ -7381,7 +7386,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 */ {
@@ -7542,14 +7548,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 */ {
@@ -7586,35 +7592,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 */ {
@@ -7626,8 +7632,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 */ {
@@ -7647,15 +7654,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 */ {
@@ -7683,7 +7690,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:
@@ -7714,16 +7721,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 */ {
@@ -7756,8 +7763,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,
 )

multibody/benchmarking/cassie.cc

@@ -1,6 +1,7 @@
 #include <memory>
 
 #include <benchmark/benchmark.h>
+#include <gflags/gflags.h>
 
 #include "drake/common/drake_assert.h"
 #include "drake/common/find_resource.h"
@@ -15,6 +16,8 @@ namespace drake {
 namespace multibody {
 namespace {
 
+DEFINE_bool(composite, false, "Enable link-combining for welded bodies.");
+
 using math::RigidTransform;
 using math::RollPitchYaw;
 using symbolic::Expression;
@@ -119,16 +122,25 @@ class Cassie : public benchmark::Fixture {
   void DoSlowSystemJacobian(benchmark::State& state) {
     DRAKE_DEMAND(want_grad_vdot(state) == false);
     DRAKE_DEMAND(want_grad_u(state) == false);
-    const int num_mobods = ssize(plant_->graph().forest().mobods());
+    const internal::SpanningForest& forest = plant_->graph().forest();
+    const int num_mobods = forest.num_mobods();
     MatrixX<double> Jv_V_WB_W(6 * num_mobods, plant_->num_velocities());
     for (auto _ : state) {
       InvalidateState();
       Jv_V_WB_W.setZero();
-      for (BodyIndex index{1}; index < plant_->num_bodies(); ++index) {
-        const Frame<double>& body_frame = plant_->get_body(index).body_frame();
-        auto J = Jv_V_WB_W.block(6 * index, 0, 6, plant_->num_velocities());
+      for (internal::MobodIndex mobod_index{1}; mobod_index < num_mobods;
+           ++mobod_index) {
+        const LinkOrdinal active_link_ordinal =
+            forest.mobods(mobod_index).active_link_ordinal();
+        const LinkIndex active_link_index =
+            forest.links(active_link_ordinal).index();
+        // The active link's link frame is also the Mobod's frame.
+        const Frame<double>& active_link_frame =
+            plant_->get_body(active_link_index).link_frame();
+        auto J =
+            Jv_V_WB_W.block(6 * mobod_index, 0, 6, plant_->num_velocities());
         plant_->CalcJacobianSpatialVelocity(
-            *context_, JacobianWrtVariable::kV, body_frame,
+            *context_, JacobianWrtVariable::kV, active_link_frame,
             Eigen::Vector3<double>::Zero(), plant_->world_frame(),
             plant_->world_frame(), &J);
       }
@@ -241,6 +253,7 @@ std::unique_ptr<MultibodyPlant<T>> Cassie<T>::MakePlant() {
   Parser parser(plant.get());
   const auto& model = "drake/multibody/benchmarking/cassie_v2.urdf";
   parser.AddModels(FindResourceOrThrow(model));
+  plant->SetFuseWeldedLinks(FLAGS_composite);
   plant->Finalize();
   if constexpr (std::is_same_v<T, double>) {
     return plant;

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/multibody_plant.cc

@@ -1649,9 +1649,7 @@ void MultibodyPlant<T>::FinalizePlantOnly() {
   if (use_sampled_output_ports_) {
     auto cache = std::make_unique<AccelerationKinematicsCache<T>>(
         internal_tree().forest());
-    for (SpatialAcceleration<T>& A_WB : cache->get_mutable_A_WB_pool()) {
-      A_WB.SetZero();
-    }
+    cache->SetToZero();
     zero_acceleration_kinematics_placeholder_ = std::move(cache);
   }
   FinalizeConstraints();