-
Notifications
You must be signed in to change notification settings - Fork 1.3k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
SapModel caches the Hessian factorization.
- Loading branch information
1 parent
2b46e26
commit d303c75
Showing
7 changed files
with
648 additions
and
3 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,89 @@ | ||
| #include "drake/multibody/contact_solvers/sap/dense_supernodal_solver.h" | ||
|
|
||
| #include <vector> | ||
|
|
||
| namespace drake { | ||
| 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_(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<Eigen::MatrixXd>& G) { | ||
| const int nv = size(); | ||
|
|
||
| // Make dense dynamics matrix. | ||
| MatrixX<double> Adense = MatrixX<double>::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<double> Jdense = J_.MakeDenseMatrix(); | ||
|
|
||
| // Make dense weight matrix G. | ||
| const int nk = Jdense.rows(); | ||
| MatrixX<double> Gdense = MatrixX<double>::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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,72 @@ | ||
| #pragma once | ||
|
|
||
| #include <vector> | ||
|
|
||
| #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<MatrixX<double>>* A, | ||
| const BlockSparseMatrix<double>* J); | ||
|
|
||
| // @returns the size of the system. | ||
| int size() const { return H_.rows(); } | ||
|
|
||
| private: | ||
| // Implementations of SuperNodalSolver NVIs. NVIs perform basic checks. | ||
| bool DoSetWeightMatrix( | ||
| const std::vector<Eigen::MatrixXd>& 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 size(); } | ||
|
|
||
| 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<Eigen::Ref<MatrixX<double>>> Hldlt_; | ||
| }; | ||
|
|
||
| } // namespace internal | ||
| } // namespace contact_solvers | ||
| } // namespace multibody | ||
| } // namespace drake |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.