Unit tests DenseSuperNodalSolver

multibody/contact_solvers/sap/BUILD.bazel

@@ -298,6 +298,15 @@ drake_cc_library(
     deps = ["//common:default_scalars"],
 )
 
+drake_cc_googletest(
+    name = "dense_supernodal_solver_test",
+    deps = [
+        ":dense_supernodal_solver",
+        "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:expect_throws_message",
+    ],
+)
+
 drake_cc_library(
     name = "validate_constraint_gradients",
     testonly = 1,

multibody/contact_solvers/sap/dense_supernodal_solver.cc

@@ -7,20 +7,37 @@ namespace multibody {
 namespace contact_solvers {
 namespace internal {
 
+int SafeGetCols(const BlockSparseMatrix<double>* J) {
+  DRAKE_THROW_UNLESS(J != nullptr);
+  return J->cols();
+}
+
+template <typename T>
+const T& SafeGetReference(std::string_view variable_name, const T* ptr) {
+  if (ptr == nullptr) {
+    throw std::runtime_error(
+        fmt::format("Condition '{} != nullptr' failed.", variable_name));
+  }
+  return *ptr;
+}
+
 DenseSuperNodalSolver::DenseSuperNodalSolver(
     const std::vector<MatrixX<double>>* A, const BlockSparseMatrix<double>* J)
-    : A_(A), J_(J) {
+    : A_(SafeGetReference("A", A)),
+      J_(SafeGetReference("J", J)),
+      H_(SafeGetCols(J), SafeGetCols(J)),
+      Hldlt_(H_) {
   DRAKE_THROW_UNLESS(A != nullptr);
   DRAKE_THROW_UNLESS(J != nullptr);
-  const int nv = [A]() {
+  const int nv = [this]() {
     int size = 0;
-    for (const auto& Ai : *A) {
+    for (const auto& Ai : A_) {
       DRAKE_THROW_UNLESS(Ai.rows() == Ai.cols());
       size += Ai.rows();
     }
     return size;
   }();
-  H_.resize(nv, nv);
+  DRAKE_DEMAND(nv == J->cols());
 }
 
 bool DenseSuperNodalSolver::DoSetWeightMatrix(
@@ -30,16 +47,16 @@ bool DenseSuperNodalSolver::DoSetWeightMatrix(
   // Make dense dynamics matrix.
   MatrixX<double> Adense = MatrixX<double>::Zero(nv, nv);
   int offset = 0;
-  for (const auto& Ac : *A_) {
+  for (const auto& Ac : A_) {
     const int nv_clique = Ac.rows();
     Adense.block(offset, offset, nv_clique, nv_clique) = Ac;
     offset += nv_clique;
   }
 
   // Make dense Jacobian matrix.
-  const MatrixX<double> Jdense = J_->MakeDenseMatrix();
+  const MatrixX<double> Jdense = J_.MakeDenseMatrix();
 
-  // Make dense Hessian matrix G.
+  // Make dense weight matrix G.
   const int nk = Jdense.rows();
   MatrixX<double> Gdense = MatrixX<double>::Zero(nk, nk);
   offset = 0;
@@ -52,6 +69,7 @@ bool DenseSuperNodalSolver::DoSetWeightMatrix(
     Gdense.block(offset, offset, ni, ni) = Gi;
     offset += ni;
   }
+  if (offset != nk) return false;  // G might miss some rows.
 
   H_ = Adense + Jdense.transpose() * Gdense * Jdense;
 

multibody/contact_solvers/sap/dense_supernodal_solver.h

@@ -20,8 +20,8 @@ class DenseSuperNodalSolver final : public SuperNodalSolver {
   // Constructs a dense solver for dynamics matrix A and Jacobian matrix J.
   // This class holds references to matrices A and J, and therefore they must
   // outlive this object.
-  // @throws std::runtime_error if A or J is nullptr.
-  // @throws std::runtime_error if a block in A is not square.
+  // @throws std::exception if A or J is nullptr.
+  // @throws std::exception if a block in A is not square.
   // @pre each block in A is SPD.
   // @pre The number of columns of J equals the size of A.
   // @note The sizes in J are not checked here, but during SetWeightMatrix().
@@ -32,24 +32,38 @@ class DenseSuperNodalSolver final : public SuperNodalSolver {
   int size() const { return H_.rows(); }
 
  private:
-  // Implementations of SuperNodalSolver NVIs.
+  // Implementations of SuperNodalSolver NVIs. NVIs perform basic checks.
   bool DoSetWeightMatrix(
       const std::vector<Eigen::MatrixXd>& block_diagonal_G) final;
-  Eigen::MatrixXd DoMakeFullMatrix() const final { return H_; }
+  Eigen::MatrixXd DoMakeFullMatrix() const final {
+    // N.B. SuperNodalSolver's NVI already checked that SetWeightMatrix() was
+    // called and the matrix was not yet factored via Factor(). Therefore no
+    // additional checks are needed here.
+    return H_;
+  }
   bool DoFactor() final;
   void DoSolveInPlace(Eigen::VectorXd* b) const final;
   int DoGetSize() const final { return size(); }
 
-  const std::vector<MatrixX<double>>* A_{nullptr};
-  const BlockSparseMatrix<double>* J_{nullptr};
+  static int CalcSize(const std::vector<MatrixX<double>>& A) {
+    int size = 0;
+    for (const auto& Ai : A) {
+      DRAKE_THROW_UNLESS(Ai.rows() == Ai.cols());
+      size += Ai.rows();
+    }
+    return size;
+  }
+
+  const std::vector<MatrixX<double>>& A_;
+  const BlockSparseMatrix<double>& J_;
   // The support for dense algebra is mostly for testing purposes, even
   // though the computation of the dense H (and in particular of the Jᵀ⋅G⋅J
   // term) is very costly. Therefore below we decided to trade off speed for
   // stability when choosing to use an LDLT decomposition instead of a slightly
   // faster, though less stable, LLT decomposition.
   MatrixX<double> H_;
   // N.B. This instantiates an in-place LDLT solver that uses the memory in H_.
-  Eigen::LDLT<MatrixX<double>> Hldlt_{H_};
+  Eigen::LDLT<Eigen::Ref<MatrixX<double>>> Hldlt_;
 };
 
 }  // namespace internal

multibody/contact_solvers/sap/sap_model.cc

@@ -42,7 +42,6 @@ std::unique_ptr<HessianFactorizationCache> HessianFactorizationCache::Clone()
     const {
   throw std::runtime_error(
       "Attempting to clone an expensive Hessian factorization.");
-  return nullptr;
 }
 
 void HessianFactorizationCache::UpdateWeightMatrixAndFactor(
@@ -60,6 +59,11 @@ void HessianFactorizationCache::SolveInPlace(
   const int num_rhs = rhs->cols();
   for (int i = 0; i < num_rhs; ++i) {
     auto rhs_i = rhs->col(i);
+    // N.B. Unfortunately SolveInPlace() only accepts VectorXd*, while here the
+    // return of col() is a block object. Therefore, we are    forced
+    // to make a copy.
+    // TODO(amcastro-tri): Update SuperNodalSolver::SolveInPlace() to accept
+    // EigenPtr instead.
     rhs_i = factorization()->Solve(rhs_i);
   }
 }
@@ -189,8 +193,8 @@ void SapModel<T>::DeclareCacheEntries() {
   system_->mutable_cache_indexes().hessian = hessian_cache_entry.cache_index();
 
   // N.B. The default constructible SapHessianFactorizationCache is empty. Since
-  // the computation of the factorization is costly, we dealy its construction
-  // to the very first time is computed.
+  // the computation of the factorization is costly, we delay its construction
+  // to the very first time it is computed.
   const auto& hessian_factorization_cache_entry = system_->DeclareCacheEntry(
       "Hessian factorization cache.",
       systems::ValueProducer(this, &SapModel<T>::CalcHessianFactorizationCache),

multibody/contact_solvers/sap/sap_model.h

@@ -131,7 +131,7 @@ class HessianFactorizationCache {
 
   // Creates an empty cache, with no heap allocation. This is used to delay the
   // creation of the expensive Hessian structure to when it is strictly needed.
-  // After instantantiations with this constructor is_empty() is `true`.
+  // After instantiations with this constructor is_empty() is `true`.
   HessianFactorizationCache() = default;
 
   // Constructor for a cache entry that stores the factorization of a SAP
@@ -142,9 +142,9 @@ class HessianFactorizationCache {
   // @note is_empty() will be `false` after construction with this constructor.
   //
   // @warning This is a potentially expensive constructor, performing the
-  // necessary symbolic analysys for the case of sparse factorizations.
+  // necessary symbolic analysis for the case of sparse factorizations.
   //
-  // @pre A and J must not be nullptr.
+  // @pre A and J are not nullptr.
   HessianFactorizationCache(SapHessianFactorizationType type,
                             const std::vector<MatrixX<double>>* A,
                             const BlockSparseMatrix<double>* J);
@@ -176,7 +176,7 @@ class HessianFactorizationCache {
   // however that cloning this object is an expensive operation that must be
   // avoided at all costs. Therefore this implementation always throws. We can
   // do this because in the well controlled lifespan of a context generated by a
-  // SapModel, we know that a HessianFactorizationCache is default construted
+  // SapModel, we know that a HessianFactorizationCache is default constructed
   // and a Clone() method is never invoked.
   std::unique_ptr<HessianFactorizationCache> Clone() const;
 
@@ -407,9 +407,13 @@ class SapModel {
   // This method evaluates the Hessian H of the primal cost ℓₚ(v).
   //
   // @note This is an expensive computation and therefore only T = double is
-  // supported.
+  // supported. When working with T = AutoDiffXd there are much more efficient
+  // and accurate ways to propagate gradients (namely implicit function theorem)
+  // than by a simple brute force propagation of gradients through the
+  // factorization itself. Therefore we discourage this practice by only
+  // providing Hessian support for T = double.
   //
-  // @trows std::runtime_error when attempting to perform the computation on T
+  // @throws std::runtime_error when attempting to perform the computation on T
   // != double.
   const HessianFactorizationCache& EvalHessianFactorizationCache(
       const systems::Context<T>& context) const {