Some minor performance tweaks & prep for M-frame algorithms.

Author: sherm1

math/cross_product.h

@@ -13,7 +13,11 @@ drake::Matrix3<typename Derived::Scalar> VectorToSkewSymmetric(
   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3);
 
   drake::Matrix3<typename Derived::Scalar> ret;
-  ret << 0.0, -p(2), p(1), p(2), 0.0, -p(0), -p(1), p(0), 0.0;
+  // clang-format off
+  ret <<  0.0, -p(2), p(1),
+          p(2), 0.0, -p(0),
+         -p(1), p(0), 0.0;
+  // clang-format on
   return ret;
 }
 

multibody/benchmarking/cassie.cc

@@ -25,21 +25,19 @@ using systems::System;
 // In the benchmark case instantiations at the bottom of this file, we'll use
 // a bitmask for the case's "Arg" to denote which quantities are in scope as
 // either gradients (for T=AutoDiffXd) or variables (for T=Expression).
-constexpr int kWantNoGrad   = 0x0;
-constexpr int kWantGradQ    = 0x1;
-constexpr int kWantGradV    = 0x2;
-constexpr int kWantGradX    = kWantGradQ | kWantGradV;
+constexpr int kWantNoGrad = 0x0;
+constexpr int kWantGradQ = 0x1;
+constexpr int kWantGradV = 0x2;
+constexpr int kWantGradX = kWantGradQ | kWantGradV;
 constexpr int kWantGradVdot = 0x4;
-constexpr int kWantGradU    = 0x8;
+constexpr int kWantGradU = 0x8;
 
 // Fixture that holds a Cassie robot model and offers helper functions to
 // configure the benchmark case.
 template <typename T>
 class Cassie : public benchmark::Fixture {
  public:
-  Cassie() {
-    tools::performance::AddMinMaxStatistics(this);
-  }
+  Cassie() { tools::performance::AddMinMaxStatistics(this); }
 
   void SetUp(benchmark::State& state) override {
     SetUpNonZeroState();
@@ -89,24 +87,28 @@ class Cassie : public benchmark::Fixture {
   // those would get computed once and re-used (like in real applications) but
   // with caching off they would get recalculated repeatedly, affecting the
   // timing results.
-  void InvalidateInput() {
-    input_.GetMutableData();
-  }
-  void InvalidateState() {
-    context_->NoteContinuousStateChange();
-  }
+  void InvalidateInput() { input_.GetMutableData(); }
+  void InvalidateState() { context_->NoteContinuousStateChange(); }
 
   // Runs the PositionKinematics benchmark.
   // NOLINTNEXTLINE(runtime/references)
   void DoPositionKinematics(benchmark::State& state) {
     DRAKE_DEMAND(want_grad_vdot(state) == false);
     DRAKE_DEMAND(want_grad_u(state) == false);
-    // A distal body. Asking for pose of one body calculates poses of all
-    // of them in the PositionKinematicsCache.
-    const RigidBody<T>& toe_right = plant_->GetBodyByName("toe_right");
     for (auto _ : state) {
       InvalidateState();
-      plant_->EvalBodyPoseInWorld(*context_, toe_right);
+      plant_->EvalPositionKinematics(*context_);
+    }
+  }
+
+  // Runs the CompositeBodyInertiaInWorld benchmark.
+  // NOLINTNEXTLINE(runtime/references)
+  void DoCompositeBodyInertiaInWorld(benchmark::State& state) {
+    DRAKE_DEMAND(want_grad_vdot(state) == false);
+    DRAKE_DEMAND(want_grad_u(state) == false);
+    for (auto _ : state) {
+      InvalidateState();
+      plant_->EvalCompositeBodyInertiaInWorldCache(*context_);
     }
   }
 
@@ -115,13 +117,21 @@ class Cassie : public benchmark::Fixture {
   void DoPosAndVelKinematics(benchmark::State& state) {
     DRAKE_DEMAND(want_grad_vdot(state) == false);
     DRAKE_DEMAND(want_grad_u(state) == false);
-    // A distal body. Asking for kinematics of one body calculates it for all
-    // of them in the PositionKinematicsCache and VelocityKinematicsCache.
-    const RigidBody<T>& toe_right = plant_->GetBodyByName("toe_right");
     for (auto _ : state) {
       InvalidateState();
       // This requires both position and velocity kinematics.
-      plant_->EvalBodySpatialVelocityInWorld(*context_, toe_right);
+      plant_->EvalVelocityKinematics(*context_);
+    }
+  }
+
+  // Runs the MassMatrixViaID benchmark.
+  // NOLINTNEXTLINE(runtime/references)
+  void DoMassMatrixViaID(benchmark::State& state) {
+    DRAKE_DEMAND(want_grad_vdot(state) == false);
+    DRAKE_DEMAND(want_grad_u(state) == false);
+    for (auto _ : state) {
+      InvalidateState();
+      plant_->CalcMassMatrixViaInverseDynamics(*context_, &mass_matrix_out_);
     }
   }
 
@@ -201,8 +211,8 @@ void Cassie<T>::SetUpNonZeroState() {
       VectorX<T>::LinSpaced(nq_ + nv_, 0.1, 0.9));
   for (const BodyIndex& index : plant_->GetFloatingBaseBodies()) {
     const RigidBody<T>& body = plant_->get_body(index);
-    const RigidTransform<T> pose(
-        RollPitchYaw<T>(0.1, 0.2, 0.3), Vector3<T>(0.4, 0.5, 0.6));
+    const RigidTransform<T> pose(RollPitchYaw<T>(0.1, 0.2, 0.3),
+                                 Vector3<T>(0.4, 0.5, 0.6));
     plant_->SetFreeBodyPose(context_.get(), body, pose);
   }
 
@@ -303,6 +313,15 @@ BENCHMARK_REGISTER_F(CassieDouble, PositionKinematics)
     ->Unit(benchmark::kMicrosecond)
     ->Arg(kWantNoGrad);
 
+BENCHMARK_DEFINE_F(CassieDouble, CompositeBodyInertiaInWorld)
+// NOLINTNEXTLINE(runtime/references)
+(benchmark::State& state) {
+  DoCompositeBodyInertiaInWorld(state);
+}
+BENCHMARK_REGISTER_F(CassieDouble, CompositeBodyInertiaInWorld)
+    ->Unit(benchmark::kMicrosecond)
+    ->Arg(kWantNoGrad);
+
 // NOLINTNEXTLINE(runtime/references)
 BENCHMARK_DEFINE_F(CassieDouble, PosAndVelKinematics)(benchmark::State& state) {
   DoPosAndVelKinematics(state);
@@ -311,29 +330,37 @@ BENCHMARK_REGISTER_F(CassieDouble, PosAndVelKinematics)
     ->Unit(benchmark::kMicrosecond)
     ->Arg(kWantNoGrad);
 
+// NOLINTNEXTLINE(runtime/references)
+BENCHMARK_DEFINE_F(CassieDouble, MassMatrixViaID)(benchmark::State& state) {
+  DoMassMatrixViaID(state);
+}
+BENCHMARK_REGISTER_F(CassieDouble, MassMatrixViaID)
+    ->Unit(benchmark::kMicrosecond)
+    ->Arg(kWantNoGrad);
+
 // NOLINTNEXTLINE(runtime/references)
 BENCHMARK_DEFINE_F(CassieDouble, MassMatrix)(benchmark::State& state) {
   DoMassMatrix(state);
 }
 BENCHMARK_REGISTER_F(CassieDouble, MassMatrix)
-  ->Unit(benchmark::kMicrosecond)
-  ->Arg(kWantNoGrad);
+    ->Unit(benchmark::kMicrosecond)
+    ->Arg(kWantNoGrad);
 
 // NOLINTNEXTLINE(runtime/references)
 BENCHMARK_DEFINE_F(CassieDouble, InverseDynamics)(benchmark::State& state) {
   DoInverseDynamics(state);
 }
 BENCHMARK_REGISTER_F(CassieDouble, InverseDynamics)
-  ->Unit(benchmark::kMicrosecond)
-  ->Arg(kWantNoGrad);
+    ->Unit(benchmark::kMicrosecond)
+    ->Arg(kWantNoGrad);
 
 // NOLINTNEXTLINE(runtime/references)
 BENCHMARK_DEFINE_F(CassieDouble, ForwardDynamics)(benchmark::State& state) {
   DoForwardDynamics(state);
 }
 BENCHMARK_REGISTER_F(CassieDouble, ForwardDynamics)
-  ->Unit(benchmark::kMicrosecond)
-  ->Arg(kWantNoGrad);
+    ->Unit(benchmark::kMicrosecond)
+    ->Arg(kWantNoGrad);
 
 BENCHMARK_DEFINE_F(CassieAutoDiff, PositionKinematics)
 // NOLINTNEXTLINE(runtime/references)
@@ -362,41 +389,41 @@ BENCHMARK_DEFINE_F(CassieAutoDiff, MassMatrix)(benchmark::State& state) {
   DoMassMatrix(state);
 }
 BENCHMARK_REGISTER_F(CassieAutoDiff, MassMatrix)
-  ->Unit(benchmark::kMicrosecond)
-  ->Arg(kWantNoGrad)
-  ->Arg(kWantGradQ)
-  ->Arg(kWantGradV)
-  ->Arg(kWantGradX);
+    ->Unit(benchmark::kMicrosecond)
+    ->Arg(kWantNoGrad)
+    ->Arg(kWantGradQ)
+    ->Arg(kWantGradV)
+    ->Arg(kWantGradX);
 
 // NOLINTNEXTLINE(runtime/references)
 BENCHMARK_DEFINE_F(CassieAutoDiff, InverseDynamics)(benchmark::State& state) {
   DoInverseDynamics(state);
 }
 BENCHMARK_REGISTER_F(CassieAutoDiff, InverseDynamics)
-  ->Unit(benchmark::kMicrosecond)
-  ->Arg(kWantNoGrad)
-  ->Arg(kWantGradQ)
-  ->Arg(kWantGradV)
-  ->Arg(kWantGradX)
-  ->Arg(kWantGradVdot)
-  ->Arg(kWantGradQ|kWantGradVdot)
-  ->Arg(kWantGradV|kWantGradVdot)
-  ->Arg(kWantGradX|kWantGradVdot);
+    ->Unit(benchmark::kMicrosecond)
+    ->Arg(kWantNoGrad)
+    ->Arg(kWantGradQ)
+    ->Arg(kWantGradV)
+    ->Arg(kWantGradX)
+    ->Arg(kWantGradVdot)
+    ->Arg(kWantGradQ | kWantGradVdot)
+    ->Arg(kWantGradV | kWantGradVdot)
+    ->Arg(kWantGradX | kWantGradVdot);
 
 // NOLINTNEXTLINE(runtime/references)
 BENCHMARK_DEFINE_F(CassieAutoDiff, ForwardDynamics)(benchmark::State& state) {
   DoForwardDynamics(state);
 }
 BENCHMARK_REGISTER_F(CassieAutoDiff, ForwardDynamics)
-  ->Unit(benchmark::kMicrosecond)
-  ->Arg(kWantNoGrad)
-  ->Arg(kWantGradQ)
-  ->Arg(kWantGradV)
-  ->Arg(kWantGradX)
-  ->Arg(kWantGradU)
-  ->Arg(kWantGradQ|kWantGradU)
-  ->Arg(kWantGradV|kWantGradU)
-  ->Arg(kWantGradX|kWantGradU);
+    ->Unit(benchmark::kMicrosecond)
+    ->Arg(kWantNoGrad)
+    ->Arg(kWantGradQ)
+    ->Arg(kWantGradV)
+    ->Arg(kWantGradX)
+    ->Arg(kWantGradU)
+    ->Arg(kWantGradQ | kWantGradU)
+    ->Arg(kWantGradV | kWantGradU)
+    ->Arg(kWantGradX | kWantGradU);
 
 BENCHMARK_DEFINE_F(CassieExpression, PositionKinematics)
 // NOLINTNEXTLINE(runtime/references)
@@ -425,38 +452,38 @@ BENCHMARK_DEFINE_F(CassieExpression, MassMatrix)(benchmark::State& state) {
   DoMassMatrix(state);
 }
 BENCHMARK_REGISTER_F(CassieExpression, MassMatrix)
-  ->Unit(benchmark::kMicrosecond)
-  ->Arg(kWantNoGrad)
-  ->Arg(kWantGradQ)
-  ->Arg(kWantGradV)
-  ->Arg(kWantGradX);
+    ->Unit(benchmark::kMicrosecond)
+    ->Arg(kWantNoGrad)
+    ->Arg(kWantGradQ)
+    ->Arg(kWantGradV)
+    ->Arg(kWantGradX);
 
 // NOLINTNEXTLINE(runtime/references)
 BENCHMARK_DEFINE_F(CassieExpression, InverseDynamics)(benchmark::State& state) {
   DoInverseDynamics(state);
 }
 BENCHMARK_REGISTER_F(CassieExpression, InverseDynamics)
-  ->Unit(benchmark::kMicrosecond)
-  ->Arg(kWantNoGrad)
-  ->Arg(kWantGradQ)
-  ->Arg(kWantGradV)
-  ->Arg(kWantGradX)
-  ->Arg(kWantGradVdot)
-  ->Arg(kWantGradQ|kWantGradVdot)
-  ->Arg(kWantGradV|kWantGradVdot)
-  ->Arg(kWantGradX|kWantGradVdot);
+    ->Unit(benchmark::kMicrosecond)
+    ->Arg(kWantNoGrad)
+    ->Arg(kWantGradQ)
+    ->Arg(kWantGradV)
+    ->Arg(kWantGradX)
+    ->Arg(kWantGradVdot)
+    ->Arg(kWantGradQ | kWantGradVdot)
+    ->Arg(kWantGradV | kWantGradVdot)
+    ->Arg(kWantGradX | kWantGradVdot);
 
 // NOLINTNEXTLINE(runtime/references)
 BENCHMARK_DEFINE_F(CassieExpression, ForwardDynamics)(benchmark::State& state) {
   DoForwardDynamics(state);
 }
 BENCHMARK_REGISTER_F(CassieExpression, ForwardDynamics)
-  ->Unit(benchmark::kMicrosecond)
-  ->Arg(kWantNoGrad)
-  // N.B. MbP does not support forward dynamics with Variables in 'q'.
-  ->Arg(kWantGradV)
-  ->Arg(kWantGradU)
-  ->Arg(kWantGradV|kWantGradU);
+    ->Unit(benchmark::kMicrosecond)
+    ->Arg(kWantNoGrad)
+    // N.B. MbP does not support forward dynamics with Variables in 'q'.
+    ->Arg(kWantGradV)
+    ->Arg(kWantGradU)
+    ->Arg(kWantGradV | kWantGradU);
 
 }  // namespace
 }  // namespace multibody

multibody/math/spatial_velocity.h

@@ -107,7 +107,7 @@ class SpatialVelocity : public SpatialVector<SpatialVelocity, T> {
   /// @see ShiftInPlace() for more information and how V_MC_E is calculated.
   SpatialVelocity<T> Shift(const Vector3<T>& offset) const {
     SpatialVelocity<T> result(*this);
-    result.ShiftInPlace(offset);
+    result.ShiftInPlace(offset);  // 12 flops
     return result;
   }
 

multibody/plant/test/external_forces_test.cc

@@ -1,14 +1,12 @@
 #include <limits>
 
-#include <gmock/gmock.h>
 #include <gtest/gtest.h>
 
-#include "drake/common/autodiff.h"
+#include "drake/common/fmt_eigen.h"
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
 #include "drake/common/test_utilities/limit_malloc.h"
 #include "drake/multibody/plant/test/kuka_iiwa_model_tests.h"
 #include "drake/multibody/tree/frame.h"
-#include "drake/multibody/tree/rigid_body.h"
 
 namespace drake {
 

multibody/plant/test/multibody_plant_mass_matrix_test.cc

@@ -23,9 +23,10 @@ namespace {
 
 // We verify the computation of the mass matrix by comparing two significantly
 // different implementations:
-//   - CalcMassMatrix(): uses the Composite Body Algorithm.
+//   - CalcMassMatrix(): Composite Body Algorithm, via recursion using World
+//     frame quantities.
 //   - CalcMassMatrixViaInverseDynamics(): uses inverse dynamics to compute each
-//     column of the mass matrix at a time.
+//     column of the mass matrix one at a time.
 class MultibodyPlantMassMatrixTests : public ::testing::Test {
  public:
   void LoadUrl(const std::string& url) {
@@ -60,30 +61,33 @@ class MultibodyPlantMassMatrixTests : public ::testing::Test {
   // mass matrix by comparing the results from CalcMassMatrix() and
   // CalcMassMatrixViaInverseDynamics().
   void VerifyMassMatrixComputation(const Context<double>& context) {
-    // Compute mass matrix via the Composite Body Algorithm.
-    MatrixX<double> Mcba(plant_.num_velocities(), plant_.num_velocities());
-    plant_.CalcMassMatrix(context, &Mcba);
+    // Compute mass matrix using the Composite Body Algorithm via recursion
+    // using World frame quantities (indicated by "_via_W" here).
+    MatrixX<double> Mcba_via_W(plant_.num_velocities(),
+                               plant_.num_velocities());
+    plant_.CalcMassMatrix(context, &Mcba_via_W);
 
     // After a first warm-up call, subsequent calls to CalcMassMatrix<double>()
     // should never allocate.
     {
       LimitMalloc guard;
-      plant_.CalcMassMatrix(context, &Mcba);
+      plant_.CalcMassMatrix(context, &Mcba_via_W);
     }
 
-    // Compute mass matrix using inverse dynamics for each column.
-    MatrixX<double> Mid(plant_.num_velocities(), plant_.num_velocities());
-    plant_.CalcMassMatrixViaInverseDynamics(context, &Mid);
+    // Compute mass matrix using inverse dynamics for each column (indicated
+    // by "_via_id" here).
+    MatrixX<double> M_via_id(plant_.num_velocities(), plant_.num_velocities());
+    plant_.CalcMassMatrixViaInverseDynamics(context, &M_via_id);
 
     // Compute a suitable tolerance scaled with the norm of the mass matrix.
     // Since .norm() computes the Frobenius norm, and num_velocities() is the
     // squared root of the number of elements in the matrix, this tolerance is
     // effectively being scaled by the RMS value of the elements in the mass
     // matrix.
-    const double kTolerance = 10.0 * std::numeric_limits<double>::epsilon() *
-                              Mcba.norm() / plant_.num_velocities();
-    EXPECT_TRUE(
-        CompareMatrices(Mcba, Mid, kTolerance, MatrixCompareType::relative));
+    const double tolerance = 10.0 * std::numeric_limits<double>::epsilon() *
+                             Mcba_via_W.norm() / plant_.num_velocities();
+    EXPECT_TRUE(CompareMatrices(Mcba_via_W, M_via_id, tolerance,
+                                MatrixCompareType::relative));
   }
 
  protected: