From 4e8a6223fa5e21f5b73a64e62c9d1511b5a5441a Mon Sep 17 00:00:00 2001 From: amcastro-tri Date: Mon, 13 May 2024 15:29:23 -0400 Subject: [PATCH] SapModel caches the Hessian factorization. --- multibody/contact_solvers/sap/BUILD.bazel | 23 ++ .../sap/dense_supernodal_solver.cc | 89 +++++++ .../sap/dense_supernodal_solver.h | 60 +++++ multibody/contact_solvers/sap/sap_model.cc | 90 ++++++- multibody/contact_solvers/sap/sap_model.h | 109 +++++++- .../sap/test/dense_supernodal_solver_test.cc | 244 ++++++++++++++++++ .../sap/test/sap_model_test.cc | 23 ++ 7 files changed, 635 insertions(+), 3 deletions(-) create mode 100644 multibody/contact_solvers/sap/dense_supernodal_solver.cc create mode 100644 multibody/contact_solvers/sap/dense_supernodal_solver.h create mode 100644 multibody/contact_solvers/sap/test/dense_supernodal_solver_test.cc diff --git a/multibody/contact_solvers/sap/BUILD.bazel b/multibody/contact_solvers/sap/BUILD.bazel index 7f7900416855..9692dba3d07d 100644 --- a/multibody/contact_solvers/sap/BUILD.bazel +++ b/multibody/contact_solvers/sap/BUILD.bazel @@ -15,6 +15,7 @@ drake_cc_package_library( visibility = ["//visibility:public"], deps = [ ":contact_problem_graph", + ":dense_supernodal_solver", ":partial_permutation", ":sap_ball_constraint", ":sap_constraint", @@ -47,6 +48,16 @@ drake_cc_library( ], ) +drake_cc_library( + name = "dense_supernodal_solver", + srcs = ["dense_supernodal_solver.cc"], + hdrs = ["dense_supernodal_solver.h"], + deps = [ + "//common:essential", + "//multibody/contact_solvers:supernodal_solver", + ], +) + drake_cc_library( name = "partial_permutation", srcs = ["partial_permutation.cc"], @@ -245,6 +256,7 @@ drake_cc_library( hdrs = ["sap_model.h"], deps = [ ":contact_problem_graph", + ":dense_supernodal_solver", ":partial_permutation", ":sap_constraint_bundle", ":sap_contact_problem", @@ -252,6 +264,8 @@ drake_cc_library( "//common:essential", "//math:linear_solve", "//multibody/contact_solvers:block_sparse_matrix", + "//multibody/contact_solvers:block_sparse_supernodal_solver", + "//multibody/contact_solvers:conex_supernodal_solver", "//systems/framework:context", "//systems/framework:leaf_system", ], @@ -284,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, diff --git a/multibody/contact_solvers/sap/dense_supernodal_solver.cc b/multibody/contact_solvers/sap/dense_supernodal_solver.cc new file mode 100644 index 000000000000..6abc6dd1b6df --- /dev/null +++ b/multibody/contact_solvers/sap/dense_supernodal_solver.cc @@ -0,0 +1,89 @@ +#include "drake/multibody/contact_solvers/sap/dense_supernodal_solver.h" + +#include + +namespace drake { +namespace multibody { +namespace contact_solvers { +namespace internal { + +int SafeGetCols(const BlockSparseMatrix* J) { + DRAKE_THROW_UNLESS(J != nullptr); + return J->cols(); +} + +template +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>* A, const BlockSparseMatrix* J) + : A_(SafeGetReference("A", A)), + J_(SafeGetReference("J", J)), + H_(SafeGetCols(J), SafeGetCols(J)), + Hldlt_(H_) { + const int nv = [this]() { + int size = 0; + for (const auto& Ai : A_) { + DRAKE_THROW_UNLESS(Ai.rows() == Ai.cols()); + size += Ai.rows(); + } + return size; + }(); + DRAKE_THROW_UNLESS(nv == J->cols()); +} + +bool DenseSuperNodalSolver::DoSetWeightMatrix( + const std::vector& G) { + const int nv = H_.rows(); + + // Make dense dynamics matrix. + MatrixX Adense = MatrixX::Zero(nv, nv); + int offset = 0; + 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 Jdense = J_.MakeDenseMatrix(); + + // Make dense weight matrix G. + const int nk = Jdense.rows(); + MatrixX Gdense = MatrixX::Zero(nk, nk); + offset = 0; + for (const auto& Gi : G) { + const int ni = Gi.rows(); + if (offset + ni > nk) { + // Weight matrix G is incompatible with the Jacobian matrix J. + return false; + } + 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; + + return true; +} + +bool DenseSuperNodalSolver::DoFactor() { + Hldlt_.compute(H_); + return Hldlt_.info() == Eigen::Success; +} + +void DenseSuperNodalSolver::DoSolveInPlace(Eigen::VectorXd* b) const { + *b = Hldlt_.solve(*b); +} + +} // namespace internal +} // namespace contact_solvers +} // namespace multibody +} // namespace drake diff --git a/multibody/contact_solvers/sap/dense_supernodal_solver.h b/multibody/contact_solvers/sap/dense_supernodal_solver.h new file mode 100644 index 000000000000..8f4aa8c6f1a4 --- /dev/null +++ b/multibody/contact_solvers/sap/dense_supernodal_solver.h @@ -0,0 +1,60 @@ +#pragma once + +#include + +#include "drake/multibody/contact_solvers/supernodal_solver.h" + +namespace drake { +namespace multibody { +namespace contact_solvers { +namespace internal { + +// A class that implements the SuperNodalSolver interface using dense algebra. +// As with every other SuperNodalSolver, this class implements a Cholesky +// factorization of a Hessian matrix H of the form H = A + Jᵀ⋅G⋅J, where A is +// referred to as the dynamics matrix and J is the Jacobian matrix. +class DenseSuperNodalSolver final : public SuperNodalSolver { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(DenseSuperNodalSolver) + + // 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::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(). + DenseSuperNodalSolver(const std::vector>* A, + const BlockSparseMatrix* J); + + private: + // Implementations of SuperNodalSolver NVIs. NVIs perform basic checks. + bool DoSetWeightMatrix( + const std::vector& block_diagonal_G) final; + 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 H_.rows(); } + + const std::vector>& A_; + const BlockSparseMatrix& 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 H_; + // N.B. This instantiates an in-place LDLT solver that uses the memory in H_. + Eigen::LDLT>> Hldlt_; +}; + +} // namespace internal +} // namespace contact_solvers +} // namespace multibody +} // namespace drake diff --git a/multibody/contact_solvers/sap/sap_model.cc b/multibody/contact_solvers/sap/sap_model.cc index 119e48fe0516..56011d016f88 100644 --- a/multibody/contact_solvers/sap/sap_model.cc +++ b/multibody/contact_solvers/sap/sap_model.cc @@ -6,7 +6,10 @@ #include "drake/common/default_scalars.h" #include "drake/math/linear_solve.h" #include "drake/multibody/contact_solvers/block_sparse_matrix.h" +#include "drake/multibody/contact_solvers/block_sparse_supernodal_solver.h" +#include "drake/multibody/contact_solvers/conex_supernodal_solver.h" #include "drake/multibody/contact_solvers/sap/contact_problem_graph.h" +#include "drake/multibody/contact_solvers/sap/dense_supernodal_solver.h" namespace drake { namespace multibody { @@ -15,9 +18,60 @@ namespace internal { using systems::Context; +HessianFactorizationCache::HessianFactorizationCache( + SapHessianFactorizationType type, const std::vector>* A, + const BlockSparseMatrix* J) { + DRAKE_DEMAND(A != nullptr); + DRAKE_DEMAND(J != nullptr); + switch (type) { + case SapHessianFactorizationType::kConex: + factorization_ = std::make_unique( + J->block_rows(), J->get_blocks(), *A); + break; + case SapHessianFactorizationType::kBlockSparseCholesky: + factorization_ = std::make_unique( + J->block_rows(), J->get_blocks(), *A); + break; + case SapHessianFactorizationType::kDense: + factorization_ = std::make_unique(A, J); + break; + } +} + +std::unique_ptr HessianFactorizationCache::Clone() + const { + throw std::runtime_error( + "Attempting to clone an expensive Hessian factorization."); +} + +void HessianFactorizationCache::UpdateWeightMatrixAndFactor( + const std::vector>& G) { + DRAKE_DEMAND(!is_empty()); + mutable_factorization()->SetWeightMatrix(G); + mutable_factorization()->Factor(); +} + +void HessianFactorizationCache::SolveInPlace( + EigenPtr> rhs) const { + DRAKE_DEMAND(!is_empty()); + // TODO(amcastro-tri): SuperNodalSolver should provide this in-place signature + // for multiple RHSs. + 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); + } +} + template -SapModel::SapModel(const SapContactProblem* problem_ptr) - : problem_(problem_ptr) { +SapModel::SapModel(const SapContactProblem* problem_ptr, + SapHessianFactorizationType hessian_type) + : problem_(problem_ptr), hessian_type_(hessian_type) { // Graph to the original contact problem, including all cliques // (participating and non-participating). const ContactProblemGraph& graph = problem().graph(); @@ -137,6 +191,17 @@ void SapModel::DeclareCacheEntries() { {system_->cache_entry_ticket( system_->cache_indexes().constraint_velocities)}); 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 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::CalcHessianFactorizationCache), + {system_->cache_entry_ticket( + system_->cache_indexes().constraint_velocities)}); + system_->mutable_cache_indexes().hessian_factorization = + hessian_factorization_cache_entry.cache_index(); } template @@ -256,6 +321,27 @@ void SapModel::CalcHessianCache(const systems::Context& context, bundle_data, &cache->impulses.gamma, &cache->G); } +template +void SapModel::CalcHessianFactorizationCache( + const systems::Context&, HessianFactorizationCache*) const { + throw std::runtime_error( + "Hessian computation is only supported for T = double"); +} + +template <> +void SapModel::CalcHessianFactorizationCache( + const systems::Context& context, + HessianFactorizationCache* hessian) const { + // Make only for the very first time. This can be an expensive computation for + // sparse Hessians even when the factorization is not yet computed. + if (hessian->is_empty()) { + *hessian = HessianFactorizationCache(hessian_type_, &dynamics_matrix(), + &constraints_bundle().J()); + } + const std::vector>& G = EvalConstraintsHessian(context); + hessian->UpdateWeightMatrixAndFactor(G); +} + template int SapModel::num_cliques() const { return problem().graph().participating_cliques().permuted_domain_size(); diff --git a/multibody/contact_solvers/sap/sap_model.h b/multibody/contact_solvers/sap/sap_model.h index 2283f9739515..18e904441521 100644 --- a/multibody/contact_solvers/sap/sap_model.h +++ b/multibody/contact_solvers/sap/sap_model.h @@ -7,6 +7,7 @@ #include "drake/multibody/contact_solvers/sap/partial_permutation.h" #include "drake/multibody/contact_solvers/sap/sap_constraint_bundle.h" #include "drake/multibody/contact_solvers/sap/sap_contact_problem.h" +#include "drake/multibody/contact_solvers/supernodal_solver.h" #include "drake/systems/framework/context.h" #include "drake/systems/framework/leaf_system.h" @@ -15,6 +16,20 @@ namespace multibody { namespace contact_solvers { namespace internal { +// Enum to specify the type of the factorization to be used by SAP. +enum class SapHessianFactorizationType { + // Supernodal KKT solver as implemented in conex [Permenter, 2020]. + // + // [Permenter, 2020] Permenter, Frank. "A geodesic interior-point method for + // linear optimization over symmetric cones." SIAM Journal on + // Optimization 33.2 (2023): 1006-1034. + kConex, + // Block sparse supernodal solver implemented by BlockSparseCholeskySolver. + kBlockSparseCholesky, + // Dense algebra. Typically used for testing. + kDense, +}; + // N.B. Ideally we'd like to nest the caching structs below into SapModel. // The structs below used for caching computations are stored by our systems:: // framework as `Value<{}>` types. Value<{}> suffers from impaired performance @@ -102,6 +117,73 @@ struct HessianCache { std::vector> G; // Constraint Hessian, G = -dγ/dvc = dP/dy⋅R⁻¹. }; +// This class stores the factorization of the Hessian matrix as a +// SuperNodalSolver, with the underlying type specified by +// SapHessianFactorizationType. +class HessianFactorizationCache { + public: + /* We allow move semantics and forbid (expensive) copy semantics. */ + HessianFactorizationCache(const HessianFactorizationCache&) = delete; + HessianFactorizationCache& operator=(const HessianFactorizationCache&) = + delete; + HessianFactorizationCache(HessianFactorizationCache&&) = default; + HessianFactorizationCache& operator=(HessianFactorizationCache&&) = default; + + // 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 instantiations with this constructor is_empty() is `true`. + HessianFactorizationCache() = default; + + // Constructor for a cache entry that stores the factorization of a SAP + // Hessian. + // This class can hold references to A and J and therefore they must outlive + // an instance of this class. + // + // @note is_empty() will be `false` after construction with this constructor. + // + // @warning This is a potentially expensive constructor, performing the + // necessary symbolic analysis for the case of sparse factorizations. + // + // @pre A and J are not nullptr. + HessianFactorizationCache(SapHessianFactorizationType type, + const std::vector>* A, + const BlockSparseMatrix* J); + + // @returns `true` if `this` factorization was never provided with a type and + // matrices A and J. + bool is_empty() const { return factorization_ == nullptr; } + + // Returns pointer to the underlying factorization. nullptr iff is_empty(). + const SuperNodalSolver* factorization() const { return factorization_.get(); } + + // Mutable pointer to the underlying factorization. nullptr iff is_empty(). + SuperNodalSolver* mutable_factorization() { return factorization_.get(); } + + // Updates the weight matrix G in H = A + Jᵀ⋅G⋅J and the factorization of H. + // @pre is_empty() is false. + void UpdateWeightMatrixAndFactor(const std::vector>& G); + + // Solves the system x = H⁻¹⋅b in place. That is, b must store the right hand + // side on input and it will store x on output. + // This signature can solve a system with multiple right hand sides, stored as + // columns of b. + // @pre is_empty() is false and UpdateWeightMatrixAndFactor() was already + // called. + void SolveInPlace(EigenPtr> b) const; + + // This Clone() implementation is provided to satisfy the "drake::Value" + // semantics needed to place this object in a cache entry for SAP. We know + // 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 constructed + // and a Clone() method is never invoked. + std::unique_ptr Clone() const; + + private: + std::unique_ptr factorization_; +}; + /* This class represents the underlying computational model built by the SAP solver given a SapContactProblem. The model re-arranges constraints to exploit the block sparse structure of the @@ -137,7 +219,9 @@ class SapModel { /* Constructs a model of `problem` optimized to be used by the SAP solver. The input `problem` must outlive `this` model. */ - explicit SapModel(const SapContactProblem* problem); + explicit SapModel(const SapContactProblem* problem, + SapHessianFactorizationType hessian_type = + SapHessianFactorizationType::kBlockSparseCholesky); /* Returns a reference to the contact problem being modeled by this class. */ const SapContactProblem& problem() const { @@ -320,6 +404,24 @@ class SapModel { .G; } + // This method evaluates the Hessian H of the primal cost ℓₚ(v). + // + // @note This is an expensive computation and therefore only T = double is + // 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. + // + // @throws std::runtime_error when attempting to perform the computation on T + // != double. + const HessianFactorizationCache& EvalHessianFactorizationCache( + const systems::Context& context) const { + return system_ + ->get_cache_entry(system_->cache_indexes().hessian_factorization) + .template Eval(context); + } + private: // Facilitate testing. friend class SapModelTester; @@ -335,6 +437,7 @@ class SapModel { systems::CacheIndex cost; systems::CacheIndex gradients; systems::CacheIndex hessian; + systems::CacheIndex hessian_factorization; systems::CacheIndex impulses; systems::CacheIndex momentum_gain; }; @@ -450,6 +553,8 @@ class SapModel { GradientsCache* cache) const; void CalcHessianCache(const systems::Context& context, HessianCache* cache) const; + void CalcHessianFactorizationCache(const systems::Context& context, + HessianFactorizationCache* hessian) const; /* Evaluates the velocity gain defined as velocity_gain = v - v*. */ const VectorX& EvalVelocityGain(const systems::Context& context) const { @@ -471,6 +576,8 @@ class SapModel { } const SapContactProblem* problem_{nullptr}; + SapHessianFactorizationType hessian_type_{ + SapHessianFactorizationType::kBlockSparseCholesky}; // TODO(amcastro-tri): Data below is heap allocated once per time step. // Consider how to pre-allocate once to minimize heap allocation. diff --git a/multibody/contact_solvers/sap/test/dense_supernodal_solver_test.cc b/multibody/contact_solvers/sap/test/dense_supernodal_solver_test.cc new file mode 100644 index 000000000000..37ccee73de0f --- /dev/null +++ b/multibody/contact_solvers/sap/test/dense_supernodal_solver_test.cc @@ -0,0 +1,244 @@ +#include "drake/multibody/contact_solvers/sap/dense_supernodal_solver.h" + +#include + +#include + +#include "drake/common/ssize.h" +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/common/unused.h" + +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::get; +using std::vector; + +namespace drake { +namespace multibody { +namespace contact_solvers { +namespace internal { + +constexpr double kEps = std::numeric_limits::epsilon(); + +// Type used to represent a block diagonal matrix in two formats: 1) a dense +// matrix and 2) a std::vector of block diagonal entries. +typedef std::pair> DenseBlockDiagonalPair; + +// Makes a block diagonal SPD matrix with three diagonal SPD blocks of size +// 2x2. +DenseBlockDiagonalPair Make6x6SpdBlockDiagonalMatrixOf2x2SpdMatrices() { + MatrixXd A(6, 6); + // clang-format off + A << 1, 1, 0, 0, 0, 0, + 1, 5, 0, 0, 0, 0, + 0, 0, 4, 1, 0, 0, + 0, 0, 1, 4, 0, 0, + 0, 0, 0, 0, 4, 2, + 0, 0, 0, 0, 2, 5; + // clang-format on + std::vector blocks(3); + blocks.at(0) = A.block<2, 2>(0, 0); + blocks.at(1) = A.block<2, 2>(2, 2); + blocks.at(2) = A.block<2, 2>(4, 4); + return std::make_pair(A, blocks); +} + +// Makes a block diagonal SPD matrix with three diagonal SPD blocks of size +// 3x3. +DenseBlockDiagonalPair Make9x9SpdBlockDiagonalMatrixOf3x3SpdMatrices() { + MatrixXd A(9, 9); + // clang-format off + A << 1, 2, 2, 0, 0, 0, 0, 0, 0, + 2, 5, 3, 0, 0, 0, 0, 0, 0, + 2, 3, 8, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 4, 1, 1, 0, 0, 0, + 0, 0, 0, 1, 4, 2, 0, 0, 0, + 0, 0, 0, 1, 2, 5, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 4, 1, 1, + 0, 0, 0, 0, 0, 0, 1, 4, 2, + 0, 0, 0, 0, 0, 0, 1, 2, 5; + // clang-format on + std::vector blocks(3); + blocks.at(0) = A.block<3, 3>(0, 0); + blocks.at(1) = A.block<3, 3>(3, 3); + blocks.at(2) = A.block<3, 3>(6, 6); + return std::make_pair(A, blocks); +} + +// Helper to make block matrix triplets for an input dense matrix A. +// block_positions[b] corresponds to the (bi, bj) block indexes of the non-zero +// b-th block. +// dense_positions[b] corresponds to the (i, j) indexes of the top-left corner +// of the b-th block in the dense matrix A. +// block_sizes[b] corresponds to the number of rows (first) and columns (second) +// for the b-th block. +std::vector MakeBlockTriplets( + const MatrixXd& A, const std::vector>& block_positions, + const std::vector>& dense_positions, + const std::vector>& block_sizes) { + DRAKE_DEMAND(block_positions.size() == dense_positions.size()); + DRAKE_DEMAND(block_positions.size() == block_sizes.size()); + const int num_blocks = ssize(block_positions); + std::vector triplets; + for (int b = 0; b < num_blocks; ++b) { + triplets.emplace_back( + block_positions[b].first, block_positions[b].second, + MatrixBlock( + A.block(dense_positions[b].first, dense_positions[b].second, + block_sizes[b].first, block_sizes[b].second))); + } + return triplets; +} + +// Verifies the correct behavior of the NVIs provided by SuperNodalSolver, +// implemented by DenseSuperNodalSolver. +GTEST_TEST(DenseSuperNodalSolver, InterfaceTest) { + const auto [A, blocks_of_A] = Make6x6SpdBlockDiagonalMatrixOf2x2SpdMatrices(); + + // Build Jacobian matrix J. + MatrixXd Jdense(9, 6); + const int num_row_blocks_of_J = 3; + const int num_col_blocks_of_J = 3; + // clang-format off + Jdense << + 1, 2, 1, 2, 0, 0, + 0, 1, 0, 1, 0, 0, + 1, 3, 1, 3, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 2, 0, + 0, 0, 0, 0, 2, 0, + 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 3; + const std::vector Jtriplets = MakeBlockTriplets(Jdense, + {{0, 0}, {1, 1}, {2, 2}}, + {{0, 0}, {3, 4}, {6, 5}}, + {{3, 4}, {3, 1}, {3, 1}}); + // clang-format on + + BlockSparseMatrixBuilder builder(num_row_blocks_of_J, + num_col_blocks_of_J, 3); + for (const auto& block : Jtriplets) { + builder.PushBlock(block.row, block.col, block.value); + } + BlockSparseMatrix J = builder.Build(); + + auto [G, blocks_of_G] = Make9x9SpdBlockDiagonalMatrixOf3x3SpdMatrices(); + + DenseSuperNodalSolver solver(&blocks_of_A, &J); + EXPECT_EQ(solver.GetSize(), 6); + + ASSERT_NO_THROW(solver.SetWeightMatrix(blocks_of_G)); + + const MatrixXd H = solver.MakeFullMatrix(); + const MatrixXd H_expected = A + Jdense.transpose() * G * Jdense; + EXPECT_TRUE( + CompareMatrices(H, H_expected, kEps, MatrixCompareType::relative)); + + EXPECT_TRUE(solver.Factor()); + VectorXd b = VectorXd::LinSpaced(A.rows(), -3.0, 12.0); + VectorXd x = solver.Solve(b); + VectorXd x_expected = H_expected.ldlt().solve(b); + EXPECT_TRUE( + CompareMatrices(x, x_expected, kEps, MatrixCompareType::relative)); +} + +GTEST_TEST(DenseSuperNodalSolver, BasicChecks) { + const auto [A, blocks_of_A] = Make6x6SpdBlockDiagonalMatrixOf2x2SpdMatrices(); + + // Build Jacobian matrix J. + MatrixXd Jdense(9, 6); + const int num_row_blocks_of_J = 3; + const int num_col_blocks_of_J = 3; + // clang-format off + Jdense << + 1, 2, 1, 2, 0, 0, + 0, 1, 0, 1, 0, 0, + 1, 3, 1, 3, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 2, 0, + 0, 0, 0, 0, 2, 0, + 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 3; + const std::vector Jtriplets = MakeBlockTriplets(Jdense, + {{0, 0}, {1, 1}, {2, 2}}, + {{0, 0}, {3, 4}, {6, 5}}, + {{3, 4}, {3, 1}, {3, 1}}); + // clang-format on + + BlockSparseMatrixBuilder builder(num_row_blocks_of_J, + num_col_blocks_of_J, 3); + for (auto& block : Jtriplets) { + builder.PushBlock(block.row, block.col, block.value); + } + BlockSparseMatrix J = builder.Build(); + + // A is nullptr. + { + auto bad_constructor_call = [&J]() { + DenseSuperNodalSolver solver(nullptr, &J); + }; + DRAKE_EXPECT_THROWS_MESSAGE(bad_constructor_call(), + "Condition 'A != nullptr' failed."); + } + + // J is nullptr. + { + // N.B. Clang doesn't like to capture blocks_of_A because its a binding. + auto bad_constructor_call = [&A_not_a_binding = blocks_of_A]() { + DenseSuperNodalSolver solver(&A_not_a_binding, nullptr); + }; + DRAKE_EXPECT_THROWS_MESSAGE(bad_constructor_call(), + "Condition 'J != nullptr' failed."); + } + + DenseSuperNodalSolver solver(&blocks_of_A, &J); + EXPECT_EQ(solver.GetSize(), 6); + + // Calling Factor() before SetWeightMatrix() throws. + { + DRAKE_EXPECT_THROWS_MESSAGE( + solver.Factor(), "Call to Factor\\(\\) failed: weight matrix not set."); + } + + auto [G, blocks_of_G] = Make9x9SpdBlockDiagonalMatrixOf3x3SpdMatrices(); + ASSERT_NO_THROW(solver.SetWeightMatrix(blocks_of_G)); + + // G is incompatible with Jacobian. + { + MatrixXd bad_block(2, 2); + // clang-format off + bad_block << 1, 1, + 1, 5; + // clang-format on + std::vector bad_G = blocks_of_G; + bad_G[1] = bad_block; + DRAKE_EXPECT_THROWS_MESSAGE(solver.SetWeightMatrix(bad_G), + "Weight matrix incompatible with Jacobian."); + } + + // Calling Solve() before Factor() throws. + VectorXd b = VectorXd::LinSpaced(A.rows(), -3.0, 12.0); + { + DRAKE_EXPECT_THROWS_MESSAGE( + solver.Solve(b), + "Call to Solve\\(\\) failed: factorization not ready."); + } + + // Calling MakeFullMatrix() after Factor() throws because the in-place + // factorization overwrites the matrix. + { + solver.Factor(); + DRAKE_EXPECT_THROWS_MESSAGE( + solver.MakeFullMatrix(), + "Call to MakeFullMatrix\\(\\) failed: weight matrix not set or matrix " + "has been factored in place."); + } +} + +} // namespace internal +} // namespace contact_solvers +} // namespace multibody +} // namespace drake diff --git a/multibody/contact_solvers/sap/test/sap_model_test.cc b/multibody/contact_solvers/sap/test/sap_model_test.cc index 0b68a261bf2f..fc103a27b4cf 100644 --- a/multibody/contact_solvers/sap/test/sap_model_test.cc +++ b/multibody/contact_solvers/sap/test/sap_model_test.cc @@ -842,6 +842,29 @@ TEST_F(DummyModelTest, CostGradients) { MatrixCompareType::relative)); } +// Unit test the computation of the Hessian factorization. +TEST_F(DummyModelTest, EvalHessianFactorization) { + const VectorXd v = arbitrary_v(); + sap_model_->SetVelocities(v, context_.get()); + + // Two arbitrary rhs. + MatrixXd b(v.size(), 2); + b << 2.5 * v, -1.2 * v; + + // Copute x = H⁻¹⋅b using the factorization. + const HessianFactorizationCache& H = + sap_model_->EvalHessianFactorizationCache(*context_); + MatrixXd x = b; + H.SolveInPlace(&x); + + // Compute expected solution. + const MatrixXd H_expected = CalcDenseHessian(); + MatrixXd x_expected = H_expected.ldlt().solve(b); + + EXPECT_TRUE(CompareMatrices(x, x_expected, 8 * kEpsilon, + MatrixCompareType::relative)); +} + } // namespace } // namespace internal } // namespace contact_solvers