[multibody] Implement kinematic results for fused links (pos & vel)

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(fuse, false, "Enable fusing for welded-together links.");
+
 using math::RigidTransform;
 using math::RollPitchYaw;
 using symbolic::Expression;
@@ -119,16 +122,21 @@ 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::MultibodyTree<double>& mbtree = GetInternalTree(*plant_);
+    const internal::SpanningForest& forest = mbtree.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 Frame<double>& active_link_frame =
+            mbtree.get_active_link(mobod_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 +249,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_fuse);
   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

@@ -1498,7 +1498,7 @@ void MultibodyPlant<T>::Finalize() {
     DeclareMiscContinuousStates();
   }
 
-  // After finalizing the base class, tree is read-only.
+  // After finalizing the base class, the tree is read-only.
   internal::MultibodyTreeSystem<T>::Finalize();
 
   if (geometry_source_is_registered()) {

multibody/plant/test/fused_welds_test.cc

@@ -1,7 +1,8 @@
-/* Tests that mass properties are identical whether welded-together links are
-modeled with unfused weld joints or whether links that are welded together
-are fused onto a mobilized body (fused Mobod). The test builds two identical
-models that differ only in whether SetFuseWeldedLinks() is enabled. */
+/* Tests that mass properties and link kinematics (poses and spatial
+velocities) are identical whether links that are welded together are modeled
+with explicit weld joints (unfused) or are fused into a single mobilized body
+(fused Mobod). The tests build two identical models that differ only in whether
+SetFuseWeldedLinks() is enabled. */
 
 #include <limits>
 #include <memory>
@@ -69,14 +70,14 @@ The positions of the link origins from World origin Wo, expressed in World are:
   Link3: (cos θ − sin θ, sin θ + cos θ, 0)
   Link4: (4, 0, 0)
 
-With fuse_welded_bodies = false, five mobilized bodies are created,
+With fuse_welded_links = false, five mobilized bodies are created,
 (World, Link1 via revolute, Link2 via weld, Link3 via weld, Link 4 via weld).
-With fuse_welded_bodies = true two mobilized bodies are created,
+With fuse_welded_links = true two mobilized bodies are created,
 (World with link4 and one fused mobilized body with Link1, Link2, Link3). */
-TestModel MakeModel(bool fuse_welded_bodies) {
+TestModel MakeModel(bool fuse_welded_links) {
   TestModel m;
   m.plant = std::make_unique<MultibodyPlant<double>>(0.0 /* continuous */);
-  m.plant->SetFuseWeldedLinks(fuse_welded_bodies);
+  m.plant->SetFuseWeldedLinks(fuse_welded_links);
 
   // To facilitate an analytical solution, each link has a trivial inertia,
   // namely a 1 kg solid cube, 0.1 m per side.
@@ -119,8 +120,8 @@ TestModel MakeModel(bool fuse_welded_bodies) {
   EXPECT_EQ(m.plant->num_positions(), 1);   // 1 revolute angle.
   EXPECT_EQ(m.plant->num_velocities(), 1);  // 1 revolute angular rate.
   const internal::MultibodyTree<double>& tree = GetInternalTree(*m.plant);
-  EXPECT_EQ(tree.num_mobods(), fuse_welded_bodies ? 2 : 5);
-  EXPECT_EQ(tree.num_mobilizers(), fuse_welded_bodies ? 2 : 5);
+  EXPECT_EQ(tree.num_mobods(), fuse_welded_links ? 2 : 5);
+  EXPECT_EQ(tree.num_mobilizers(), fuse_welded_links ? 2 : 5);
   // Note: num_mobilizers() == num_mobods() because the World body gets a dummy
   // weld mobilizer at index 0 to keep mobilizer/body-node indexing identical.
 
@@ -139,7 +140,7 @@ TestModel MakeModel(bool fuse_welded_bodies) {
   // Ensure Mobods (mobilized bodies) have the proper follower link ordinals.
   // Each Mobod should have an active link ordinal. If welded links have
   // been fused, then there are additional follower link ordinals.
-  if (fuse_welded_bodies) {
+  if (fuse_welded_links) {
     EXPECT_EQ(mobod_0.follower_link_ordinals(),
               (std::vector{LinkOrdinal(0), LinkOrdinal(4)}));
     EXPECT_EQ(mobod_1.follower_link_ordinals(),
@@ -157,12 +158,186 @@ void SetState(const TestModel& m, double angle_rad, double angular_vel) {
   m.revolute->set_angular_rate(m.context.get(), angular_vel);
 }
 
+/* Tests that link poses and spatial velocities are identical whether welds are
+modeled explicitly (unfused) or combined into a fused mobilized body. This
+doesn't check that the values are right, just that they match. Other tests
+below will check some numerical results. */
+GTEST_TEST(CompositeTest, LinkKinematicsMatchExplicitWelds) {
+  const TestModel unfused_model = MakeModel(false);
+  const TestModel fused_model = MakeModel(true);
+
+  // Test at several non-trivial (q, v) configurations.
+  const std::vector<std::pair<double, double>> configs = {
+      {0.0, 0.0},      {M_PI / 6, 0.5},  {M_PI / 4, -1.2},
+      {M_PI / 2, 2.0}, {-M_PI / 3, 0.7},
+  };
+
+  for (const auto& [angle, omega] : configs) {
+    SetState(unfused_model, angle, omega);
+    SetState(fused_model, angle, omega);
+
+    for (const auto& [link_name, unfused_link, fused_link] :
+         std::vector<std::tuple<const char*, const RigidBody<double>*,
+                                const RigidBody<double>*>>{
+             {"Link1", unfused_model.link1, fused_model.link1},
+             {"Link2", unfused_model.link2, fused_model.link2},
+             {"Link3", unfused_model.link3, fused_model.link3},
+             {"Link4", unfused_model.link4, fused_model.link4},
+         }) {
+      // Compare world poses.
+      const RigidTransformd& X_WL_unfused =
+          unfused_link->EvalPoseInWorld(*unfused_model.context);
+      const RigidTransformd& X_WL_fused =
+          fused_link->EvalPoseInWorld(*fused_model.context);
+
+      EXPECT_TRUE(X_WL_unfused.IsNearlyEqualTo(X_WL_fused, kTolerance));
+
+      // Compare world spatial velocities.
+      const SpatialVelocity<double>& V_WL_unfused =
+          unfused_link->EvalSpatialVelocityInWorld(*unfused_model.context);
+      const SpatialVelocity<double>& V_WL_fused =
+          fused_link->EvalSpatialVelocityInWorld(*fused_model.context);
+
+      EXPECT_TRUE(CompareMatrices(V_WL_unfused.get_coeffs(),
+                                  V_WL_fused.get_coeffs(), kTolerance,
+                                  MatrixCompareType::relative));
+    }
+  }
+}
+
+/* Test the pose of each link at the zero configuration. */
+GTEST_TEST(CompositeTest, ZeroConfigurationPoses) {
+  for (bool fuse : {false, true}) {
+    SCOPED_TRACE(fuse ? "fused" : "unfused");
+    const TestModel m = MakeModel(fuse);
+    SetState(m, 0.0, 0.0);
+
+    // Verify Link1 is at World origin, no rotation.
+    const RigidTransformd& X_WL1 = m.link1->EvalPoseInWorld(*m.context);
+    EXPECT_TRUE(X_WL1.IsNearlyEqualTo(RigidTransformd::Identity(), kTolerance));
+
+    // Verify Link2's position in World is (1, 0, 0), no rotation.
+    const RigidTransformd& X_WL2 = m.link2->EvalPoseInWorld(*m.context);
+    EXPECT_TRUE(X_WL2.IsNearlyEqualTo(
+        RigidTransformd(Vector3<double>(1.0, 0.0, 0.0)), kTolerance));
+
+    // Verify Link3's position in World is (1, 1, 0), no rotation.
+    const RigidTransformd& X_WL3 = m.link3->EvalPoseInWorld(*m.context);
+    EXPECT_TRUE(X_WL3.IsNearlyEqualTo(
+        RigidTransformd(Vector3<double>(1.0, 1.0, 0.0)), kTolerance));
+
+    // Verify Link4's position in World is (4, 0, 0), no rotation.
+    const RigidTransformd& X_WL4 = m.link4->EvalPoseInWorld(*m.context);
+    EXPECT_TRUE(X_WL4.IsNearlyEqualTo(
+        RigidTransformd(Vector3<double>(4.0, 0.0, 0.0)), kTolerance));
+  }
+}
+
+/* Test the pose of each link when Link1 is rotated 90°. */
+GTEST_TEST(CompositeTest, NinetyDegreePoses) {
+  for (bool fuse : {false, true}) {
+    SCOPED_TRACE(fuse ? "fused" : "unfused");
+    const TestModel m = MakeModel(fuse);
+    SetState(m, M_PI / 2, 0.0);
+
+    // Link2 and Link3 share Link1's 90° z-rotation.
+    const math::RotationMatrixd R_W90z =
+        math::RotationMatrixd::MakeZRotation(M_PI / 2);
+
+    // Verify Link1 has a 90° z-rotation relative to World
+    // and Link1's origin is at World origin.
+    const RigidTransformd& X_WL1 = m.link1->EvalPoseInWorld(*m.context);
+    EXPECT_TRUE(X_WL1.IsNearlyEqualTo(
+        RigidTransformd(R_W90z, Vector3<double>::Zero()), kTolerance));
+
+    // Link2's origin is (1, 0, 0) along Link1's +x direction. Link1's +x
+    // direction points in World's +y direction due to the 90° z-rotation, so
+    // verify Link2's origin in World is (0, 1, 0).
+    const RigidTransformd& X_WL2 = m.link2->EvalPoseInWorld(*m.context);
+    EXPECT_TRUE(X_WL2.IsNearlyEqualTo(
+        RigidTransformd(R_W90z, Vector3<double>(0.0, 1.0, 0.0)), kTolerance));
+
+    // Link3's origin is 1 m along Link2's y-axis. After 90° rotation, Link2's
+    // y-axis points in World -x, so Link3 is at (0,1,0) + (-1,0,0) = (-1,1,0).
+    const RigidTransformd& X_WL3 = m.link3->EvalPoseInWorld(*m.context);
+    EXPECT_TRUE(X_WL3.IsNearlyEqualTo(
+        RigidTransformd(R_W90z, Vector3<double>(-1.0, 1.0, 0.0)), kTolerance));
+
+    // Verify Link4's position in World is (4, 0, 0), no rotation.
+    const RigidTransformd& X_WL4 = m.link4->EvalPoseInWorld(*m.context);
+    EXPECT_TRUE(X_WL4.IsNearlyEqualTo(
+        RigidTransformd(Vector3<double>(4.0, 0.0, 0.0)), kTolerance));
+  }
+}
+
+/* Verify spatial velocities for individual links, regardless of whether
+welded links are fused. */
+GTEST_TEST(CompositeTest, SpatialVelocities) {
+  const double angle = M_PI / 4;
+  const double omega = 2.0;
+
+  for (bool fuse : {false, true}) {
+    SCOPED_TRACE(fuse ? "fused" : "unfused");
+    const TestModel m = MakeModel(fuse);
+    SetState(m, angle, omega);
+
+    // Link2 and Link3 share Link1's angular velocity in world of ω * ẑ.
+    const Vector3<double> w_expected = omega * Vector3<double>::UnitZ();
+    for (const RigidBody<double>* link : {m.link1, m.link2, m.link3}) {
+      const SpatialVelocity<double>& V_WL =
+          link->EvalSpatialVelocityInWorld(*m.context);
+      EXPECT_TRUE(CompareMatrices(V_WL.rotational(), w_expected, kTolerance,
+                                  MatrixCompareType::relative));
+    }
+
+    // Verify that the translational velocity of L1o (Link1's origin) in World
+    // is zero (L1o is at World origin).
+    const SpatialVelocity<double>& V_WL1o =
+        m.link1->EvalSpatialVelocityInWorld(*m.context);
+    EXPECT_TRUE(CompareMatrices(V_WL1o.translational(), Vector3<double>::Zero(),
+                                kTolerance, MatrixCompareType::relative));
+
+    // Verify that the translational velocity of L2o (Link2's origin) in World
+    // is ω × p_WL2o.
+    // p_WL2o = R_W_L1 * [1, 0, 0] = [cos θ, sin θ, 0].
+    // v_WL2o = ω ẑ × [cos θ, sin θ, 0] = ω[-sin θ, cos θ, 0].
+    const Vector3<double> v_WL2o_expected =
+        omega * Vector3<double>(-std::sin(angle), std::cos(angle), 0.0);
+    const SpatialVelocity<double>& V_WL2o =
+        m.link2->EvalSpatialVelocityInWorld(*m.context);
+    EXPECT_TRUE(CompareMatrices(V_WL2o.translational(), v_WL2o_expected,
+                                kTolerance, MatrixCompareType::relative));
+
+    // Verify that the translational velocity of L3o (Link3's origin) in World
+    // is ω × p_WL3o.
+    // p_WL3o = R_WL1 * [1,0,0] + R_WL1 * [0,1,0]
+    //        = [cos θ - sin θ, sin θ + cos θ, 0].
+    // v_WL3o = ω ẑ × p_WL3o
+    //        = ω ẑ × [cos θ - sin θ, sin θ + cos θ, 0]
+    //        = ω[-(sin θ + cos θ), cos θ - sin θ, 0].
+    const Vector3<double> v_WL3o_expected =
+        omega * Vector3<double>(-(std::sin(angle) + std::cos(angle)),
+                                std::cos(angle) - std::sin(angle), 0.0);
+    const SpatialVelocity<double>& V_WL3o =
+        m.link3->EvalSpatialVelocityInWorld(*m.context);
+    EXPECT_TRUE(CompareMatrices(V_WL3o.translational(), v_WL3o_expected,
+                                kTolerance, MatrixCompareType::relative));
+
+    // Verify Link4's is welded to World (zero spatial velocity).
+    const SpatialVelocity<double>& V_WL4o =
+        m.link4->EvalSpatialVelocityInWorld(*m.context);
+    EXPECT_TRUE(CompareMatrices(V_WL4o.translational(), Vector3<double>::Zero(),
+                                kTolerance, MatrixCompareType::relative));
+  }
+}
+
 /* Verify that the spatial inertia for fused mobod Link123 (with links 1, 2, 3)
-matches the sum of spatial inertias for unfused links 1, 2, 3 at several poses.
-Next, ensure spatial inertia for each of the follower links in a fused mobod are
-computed correctly. Lastly, the 1x1 mass matrix for this 1-DOF model directly
-depends on the spatial inertia of Link123, so we also verify:
-  (a) The mass matrix is identical between the fused and unfused weld models.
+matches the sum of spatial inertias for unfused links 1, 2, 3. Next, ensure
+spatial inertia for each of the follower links in a fused mobod are computed
+correctly. Lastly, the 1x1 mass matrix for this 1-DOF model directly depends on
+the spatial inertia of Link123, so we also verify:
+  (a) The mass matrix is identical between the unfused and fused
+      models at several configurations.
   (b) The mass matrix matches the analytically computed value.
 
 Similarly, verify spatial momentum calculations.
@@ -173,24 +348,24 @@ The model has 4 links, Linki (i=1,2,3,4), each a 1 kg solid cube of side 0.1 m.
 Links 1,2,3 are welded together and are connected to World via a revolute joint
 (z-axis at World origin). Link4 is welded directly to World. The link body-frame
 origins are coincident with their associated centers of mass, located with:
-  Link1 origin from World origin: p₁ = (0, 0, 0) expressed in World.
-  Link2 origin from Link1 origin: p₂ = (1, 0, 0) expressed in Link1.
-  Link3 origin from Link1 origin: p₃ = (1, 1, 0) expressed in Link1.
-  Link4 origin from World origin: p₄ = (4, 0, 0) expressed in World.
+ Link1 origin from World origin: p₁ = (0, 0, 0) expressed in World.
+ Link2 origin from Link1 origin: p₂ = (1, 0, 0) expressed in Link1.
+ Link3 origin from Link1 origin: p₃ = (1, 1, 0) expressed in Link1.
+ Link4 origin from World origin: p₄ = (4, 0, 0) expressed in World.
 
 For a solid cube of mass m and side a, its moment of inertia about any axis
 through its COM is m*a²/6. The parallel axis theorem calculates each cube's
 moment of inertia about the revolute's z-axis via: Iᵢ = m*a²/6 + m*(dᵢ)²,
 where dᵢ (i=1,2,3) is the distance between each cube's COM and the revolute's
 z-axis. Iᵢ is independent of joint angle because the links are welded together.
 
-  Link1: I₁ = 1*(0.1)²/6 + 1*0²    = 1/600 + 0   (d² = 0)
-  Link2: I₂ = 1*(0.1)²/6 + 1*1²    = 1/600 + 1   (d² = 1)
-  Link3: I₃ = 1*(0.1)²/6 + 1*√2²   = 1/600 + 2   (d² = 2)
-  Total: Iₜ = 3/600 + 3 = 1/200 + 3 = 3.005 kg·m² */
+ Link1: I₁ = 1*(0.1)²/6 + 1*0²    = 1/600 + 0   (d² = 0)
+ Link2: I₂ = 1*(0.1)²/6 + 1*1²    = 1/600 + 1   (d² = 1)
+ Link3: I₃ = 1*(0.1)²/6 + 1*√2²   = 1/600 + 2   (d² = 2)
+ Total: Iₜ = 3/600 + 3 = 1/200 + 3 = 3.005 kg·m² */
 GTEST_TEST(FusedTest, CompositeSpatialInertia) {
-  const TestModel unfused_model = MakeModel(false /* no combining */);
-  const TestModel fused_model = MakeModel(true /* fuse welds */);
+  const TestModel unfused_model = MakeModel(false);
+  const TestModel fused_model = MakeModel(true);
 
   const double m = 1.0, a = 0.1;  // mass m and side-length a of solid cubes.
   const Frame<double>& world_frame = unfused_model.plant->world_frame();