[ad] Store (scaled) unit vectors inline

bindings/generated_docstrings/geometry.h

@@ -5814,7 +5814,7 @@ throwsᵈ | throwsᵈ | ░░░░░ | ░░░░░░░ | ░░░░
 throwsᵈ | throwsᵈ | throwsᵈ | ░░░░░░ | ░░░░░ | ░░░░░ | | HalfSpace |
 throwsᵈ | throwsᵈ | throwsᵈ | throwsᵈ | throwsᵈ | throwsᵃ | ░░░░░ |
 ░░░░░ | | Mesh | ᵇ | ᵇ | ᵇ | ᵇ | ᵇ | ᵇ | ᵇ | ░░░░░ | | Sphere | 2e-15
-| 3e-15 | throwsᵈ | 2e-15 | throwsᵈ | 2e-15 | ᵇ | 5e-15 | ***Table
+| 5e-15 | throwsᵈ | 2e-15 | throwsᵈ | 2e-15 | ᵇ | 5e-15 | ***Table
 2***: Worst observed error (in m) for 2mm penetration between
 geometries approximately 20cm in size for ``T`` = drake∷AutoDiffXd
 "AutoDiffXd".
@@ -6141,7 +6141,7 @@ as a zero-radius sphere.
 Mesh | Sphere | | :--------: | :-----: | :------: | :-----: |
 :-------: | :--------: | :--------: | :-----: | :-----: | | double |
 2e-15 | 4e-15 | 6e-15 | 3e-15 | 3e-5ᵇ | 5e-15 | 6e-15ᶜ | 4e-15 | |
-AutoDiffXd | 1e-15 | 7e-15 | ᵃ | ᵃ | ᵃ | 5e-15 | ᵃ | 3e-15 | |
+AutoDiffXd | 1e-15 | 7e-15 | ᵃ | ᵃ | ᵃ | 5e-15 | ᵃ | 4e-15 | |
 Expression | ᵃ | ᵃ | ᵃ | ᵃ | ᵃ | ᵃ | ᵃ | ᵃ | ***Table 8***: Worst
 observed error (in m) for 2mm penetration/separation between geometry
 approximately 20cm in size and a point.

common/ad/BUILD.bazel

@@ -20,6 +20,7 @@ drake_cc_library(
     name = "auto_diff",
     srcs = [
         "internal/derivatives_xpr.cc",
+        "internal/eigen_specializations.cc",
         "internal/partials.cc",
         "internal/standard_operations.cc",
     ],
@@ -59,6 +60,14 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "matrix_heap_test",
+    deps = [
+        ":auto_diff",
+        "//common/test_utilities:limit_malloc",
+    ],
+)
+
 drake_cc_googletest(
     name = "partials_test",
     deps = [
@@ -72,7 +81,7 @@ drake_cc_googletest(
 drake_cc_googletest(
     name = "partials_heap_test",
     deps = [
-        "//common/ad:auto_diff",
+        ":auto_diff",
         "//common/test_utilities:limit_malloc",
     ],
 )

common/ad/auto_diff.h

@@ -14,8 +14,9 @@ namespace ad {
 Drake's AutoDiff is not templated; it only supports dynamically-sized
 derivatives using floating-point doubles.
 
-However, by using a careful representation trick (maintaining a separate scale
-factor) it runs faster than `Eigen::AutoDiffScalar<Eigen::VectorXd>`. */
+However, by using careful representation tricks (maintaining a separate scale
+factor, and using inline storage in case only one partial is non-zero) it runs
+faster than `Eigen::AutoDiffScalar<Eigen::VectorXd>`. */
 class AutoDiff {
  public:
   DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(AutoDiff);

common/ad/internal/eigen_specializations.cc

@@ -0,0 +1,53 @@
+#include "drake/common/ad/auto_diff.h"
+
+namespace drake {
+namespace ad {
+namespace internal {
+namespace {
+
+AutoDiff GenericGevv(const Eigen::Ref<const VectorX<AutoDiff>, 0, StrideX>& a,
+                     const Eigen::Ref<const VectorX<AutoDiff>, 0, StrideX>& b) {
+  DRAKE_ASSERT(a.size() == b.size());
+  DRAKE_ASSERT(a.size() > 0);
+  AutoDiff result = a.coeffRef(0) * b.coeffRef(0);
+  for (int i = 1; i < a.size(); ++i) {
+    const AutoDiff& a_i = a.coeffRef(i);
+    const AutoDiff& b_i = b.coeffRef(i);
+    // Spell out `result += a_i * b_i` with a combined multiply-accumulate.
+    // ∂/∂x result + (a * b) = result' + (a * b)' = result' + ba' + ab'
+    result.partials().AddScaled(b_i.value(), a_i.partials());
+    result.partials().AddScaled(a_i.value(), b_i.partials());
+    result.value() += a_i.value() * b_i.value();
+  }
+  return result;
+}
+
+}  // namespace
+
+void Gemm(const Eigen::Ref<const MatrixX<AutoDiff>, 0, StrideX>& left,
+          const Eigen::Ref<const MatrixX<AutoDiff>, 0, StrideX>& right,
+          EigenPtr<MatrixX<AutoDiff>> result) {
+  // These checks are guaranteed by our header file functions that call us.
+  DRAKE_ASSERT(result != nullptr);
+  DRAKE_ASSERT(result->rows() == left.rows());
+  DRAKE_ASSERT(result->cols() == right.cols());
+  DRAKE_ASSERT(left.cols() == right.rows());
+
+  // Multiplying Mx0 matrix * 0xN matrix produces a MxN zero matrix without any
+  // partials.
+  if (left.cols() == 0) {
+    result->setZero();
+    return;
+  }
+
+  // Delegate to Gevv.
+  for (int i = 0; i < result->rows(); ++i) {
+    for (int j = 0; j < result->cols(); ++j) {
+      (*result)(i, j) = GenericGevv(left.row(i), right.col(j));
+    }
+  }
+}
+
+}  // namespace internal
+}  // namespace ad
+}  // namespace drake

common/ad/internal/eigen_specializations.h

@@ -4,9 +4,14 @@
 
 #include <Eigen/Core>
 
+#include "drake/common/drake_assert.h"
+
 /* This file contains Eigen-related specializations for Drake's AutoDiff
 scalar type (plus the intimately related std::numeric_limits specialization).
 
+The specializations both add basic capability (e.g., NumTraits) as well as
+improve performance (e.g., fewer copies than Eigen would naively use).
+
 NOTE: This file should never be included directly, rather only from
 auto_diff.h in a very specific order. */
 
@@ -56,3 +61,67 @@ struct ScalarBinaryOpTraits<double, drake::ad::AutoDiff, BinOp> {
 }  // namespace Eigen
 
 #endif  // DRAKE_DOXYGEN_CXX
+
+namespace drake {
+namespace ad {
+namespace internal {
+
+/* A specialized implementation of BLAS GEMM for AutoDiff that is faster than
+Eigen's generic implementation. With our current mechanism for hooking this into
+Eigen, we only need to support the simplified form C ⇐ A@B rather than the more
+general C ⇐ αA@B+βC of typical GEMM; if we figure out how to hook into Eigen's
+expression templates, we could expand to the more general form.
+
+The dynamic StrideX allows for passing numpy.ndarray without copies. This
+function isn't yet bound / used in pydrake, but might be in the future.
+
+Sets result = left * right. */
+void Gemm(const Eigen::Ref<const MatrixX<AutoDiff>, 0, StrideX>& left,
+          const Eigen::Ref<const MatrixX<AutoDiff>, 0, StrideX>& right,
+          EigenPtr<MatrixX<AutoDiff>> result);
+
+/* Eigen promises "automatic conversion of the inner product to a scalar", so
+when we calculate an inner product we need to return this magic type that acts
+like both a Matrix1<AutoDiff> and a scalar AutoDiff. There does not appear to be
+any practical way to mimic what Eigen does, other than multiple inheritance. */
+class DegenerateAutoDiffInnerProduct
+    : public AutoDiff,
+      public Eigen::Map<Eigen::Matrix<AutoDiff, 1, 1>> {
+ public:
+  DegenerateAutoDiffInnerProduct()
+      // Because Map here expects an AutoDiff* the `this` pointer to the just-
+      // constructed object converts to point to the AutoDiff base class object
+      // so that both the Map matrix and the AutoDiff scalar use the same
+      // memory.
+      : Map(this) {}
+};
+
+/* Helper to look up the return type we'll use for an AutoDiff matmul. */
+template <typename MatrixL, typename MatrixR>
+using AutoDiffMatMulResult =
+    std::conditional_t<MatrixL::RowsAtCompileTime == 1 &&
+                           MatrixR::ColsAtCompileTime == 1,
+                       DegenerateAutoDiffInnerProduct,
+                       Eigen::Matrix<AutoDiff, MatrixL::RowsAtCompileTime,
+                                     MatrixR::ColsAtCompileTime>>;
+
+}  // namespace internal
+
+// Matrix<AutoDiff> * Matrix<AutoDiff> => Matrix<AutoDiff>
+template <typename MatrixL, typename MatrixR>
+internal::AutoDiffMatMulResult<MatrixL, MatrixR> operator*(const MatrixL& lhs,
+                                                           const MatrixR& rhs)
+  requires std::is_base_of_v<Eigen::MatrixBase<MatrixL>, MatrixL> &&
+           std::is_base_of_v<Eigen::MatrixBase<MatrixR>, MatrixR> &&
+           std::is_same_v<typename MatrixL::Scalar, AutoDiff> &&
+           std::is_same_v<typename MatrixR::Scalar, AutoDiff>
+{
+  internal::AutoDiffMatMulResult<MatrixL, MatrixR> result;
+  DRAKE_THROW_UNLESS(lhs.cols() == rhs.rows());
+  result.resize(lhs.rows(), rhs.cols());
+  internal::Gemm(lhs, rhs, &result);
+  return result;
+}
+
+}  // namespace ad
+}  // namespace drake