Replace LDLT in multibody plant with math::LinearSolver (RobotLocomot…

…ion#15874)

* Replace LDLT in multibody plant with math::LinearSolver

The math::LinearSolver is a lot faster than templated Eigen solver when
the scalar type is AutoDiffScalar.
Add a default constructor for LinearSolver class.
Change the return type of LinearSolver::Solve(). If both A and b contain
the same scalar type (either double or symbolic::Expression), then the
return type is Eigen::Solve<>, the same return type as
decomposition.solve().

examples/multibody/cassie_benchmark/test/cassie_bench.cc

@@ -287,7 +287,7 @@ BENCHMARK_F(CassieAutodiffFixture, AutodiffForwardDynamics)
     // mark comes when building against Eigen 3.4, so only tighten the limit
     // here if you've tested vs Eigen 3.4 specifically -- our default build
     // of Drake currently uses the Eigen 3.3 series.
-    LimitMalloc guard(LimitReleaseOnly(57787));
+    LimitMalloc guard(LimitReleaseOnly(57916));
 
     compute();
 

math/linear_solve.h

@@ -639,11 +639,31 @@ template <template <typename, int...> typename LinearSolverType,
           typename DerivedA>
 class LinearSolver {
  public:
+  using SolverType = internal::EigenLinearSolver<LinearSolverType, DerivedA>;
+
+  /**
+   * The return type of Solve() function.
+   * When both A and B contain the same scalar, and that scalar type
+   * is double or symbolic::Expression, then the return type is
+   * Eigen::Solve<Decomposition, DerivedB>, same as the return type of
+   * Decomposition::solve() function in Eigen. This avoids unnecessary copies
+   * and heap memory allocations if we were to evaluate
+   * Eigen::Solve<Decomposition, DerivedB> to a concrete Eigen::Matrix type.
+   * Othewise we return an Eigen::Matrix with the same size as DerivedB
+   * and proper scalar type.
+   */
   template <typename DerivedB>
-  using SolutionType = internal::Solution<DerivedA, DerivedB>;
+  using SolutionType = std::conditional_t<
+      std::is_same_v<typename DerivedA::Scalar, typename DerivedB::Scalar> &&
+          internal::is_double_or_symbolic_v<typename DerivedA::Scalar>,
+      Eigen::Solve<SolverType, DerivedB>,
+      internal::Solution<DerivedA, DerivedB>>;
+
+  /** Default constructor. Constructs an empty linear solver. */
+  LinearSolver() : eigen_linear_solver_() {}
 
   explicit LinearSolver(const Eigen::MatrixBase<DerivedA>& A)
-      : linear_solver_{GetLinearSolver<LinearSolverType>(A)} {
+      : eigen_linear_solver_{GetLinearSolver<LinearSolverType>(A)} {
     if constexpr (internal::is_autodiff_v<typename DerivedA::Scalar>) {
       A_.emplace(A);
     }
@@ -664,21 +684,38 @@ class LinearSolver {
 
     if constexpr (std::is_same_v<ScalarA, ScalarB> &&
                   !internal::is_autodiff_v<ScalarA>) {
-      return linear_solver_.solve(b);
+      return eigen_linear_solver_.solve(b);
       // NOLINTNEXTLINE(readability/braces)
     } else if constexpr (std::is_same_v<ScalarA, double> &&
                          internal::is_autodiff_v<ScalarB>) {
-      return SolveLinearSystem(linear_solver_, b);
+      return SolveLinearSystem(eigen_linear_solver_, b);
       // NOLINTNEXTLINE(readability/braces)
     } else if constexpr (internal::is_autodiff_v<ScalarA>) {
-      return SolveLinearSystem(linear_solver_, *A_, b);
+      return SolveLinearSystem(eigen_linear_solver_, *A_, b);
     }
     DRAKE_UNREACHABLE();
   }
 
+  /** Getter for the Eigen linear solver.
+   * The scalar type in the Eigen linear solver depends on the scalar type in A
+   * matrix, as shown in this table
+   * |      A       | double |  ADS   | Expr |
+   * |--------------|--------|--------|----- |
+   * |linear_solver | double | double | Expr |
+   *
+   * where ADS stands for Eigen::AutoDiffScalar, Expr stands for
+   * symbolic::Expression.
+   *
+   * Note that when A contains autodiffscalar, we only use the double version of
+   * Eigen linear solver. By using implicit-function theorem with the
+   * double-valued Eigen linear solver, we can compute the gradient of the
+   * solution much faster than directly autodiffing the Eigen linear solver.
+   *
+   */
+  const SolverType& eigen_linear_solver() const { return eigen_linear_solver_; }
+
  private:
-  using SolverType = internal::EigenLinearSolver<LinearSolverType, DerivedA>;
-  SolverType linear_solver_;
+  SolverType eigen_linear_solver_;
   std::optional<Eigen::Matrix<
       typename DerivedA::Scalar, DerivedA::RowsAtCompileTime,
       DerivedA::ColsAtCompileTime, Eigen::ColMajor,

multibody/plant/multibody_plant.cc

@@ -2543,10 +2543,9 @@ void MultibodyPlant<T>::CallContactSolver(
       : public contact_solvers::internal::LinearOperator<T> {
    public:
     MassMatrixInverseOperator(const std::string& name, const MatrixX<T>* M)
-        : contact_solvers::internal::LinearOperator<T>(name) {
+        : contact_solvers::internal::LinearOperator<T>(name), M_ldlt_{*M} {
       DRAKE_DEMAND(M != nullptr);
       nv_ = M->rows();
-      M_ldlt_ = M->ldlt();
       // TODO(sherm1) Eliminate heap allocation.
       tmp_.resize(nv_);
     }
@@ -2559,15 +2558,15 @@ void MultibodyPlant<T>::CallContactSolver(
     void DoMultiply(const Eigen::Ref<const Eigen::SparseVector<T>>& x,
                     Eigen::SparseVector<T>* y) const final {
       tmp_ = VectorX<T>(x);
-      *y = M_ldlt_.solve(tmp_).sparseView();
+      *y = M_ldlt_.Solve(tmp_).sparseView();
     }
     void DoMultiply(const Eigen::Ref<const VectorX<T>>& x,
                     VectorX<T>* y) const final {
-      *y = M_ldlt_.solve(x);
+      *y = M_ldlt_.Solve(x);
     }
     int nv_;
     mutable VectorX<T> tmp_;  // temporary workspace.
-    Eigen::LDLT<MatrixX<T>> M_ldlt_;
+    math::LinearSolver<Eigen::LDLT, MatrixX<T>> M_ldlt_;
   };
   MassMatrixInverseOperator Minv_op("Minv", &M0);
 

multibody/tree/BUILD.bazel

@@ -242,6 +242,7 @@ drake_cc_library(
         "//common:essential",
         "//common:nice_type_name",
         "//common:symbolic",
+        "//math:linear_solve",
         "//math:vector3_util",
         "//multibody/math:spatial_algebra",
     ],

multibody/tree/articulated_body_inertia_cache.h

@@ -5,6 +5,7 @@
 
 #include "drake/common/default_scalars.h"
 #include "drake/common/drake_copyable.h"
+#include "drake/math/linear_solve.h"
 #include "drake/multibody/tree/articulated_body_inertia.h"
 #include "drake/multibody/tree/multibody_tree_indexes.h"
 #include "drake/multibody/tree/multibody_tree_topology.h"
@@ -73,14 +74,14 @@ class ArticulatedBodyInertiaCache {
   }
 
   // LDLT factorization `ldlt_D_B` of the articulated body hinge inertia.
-  const Eigen::LDLT<MatrixUpTo6<T>>& get_ldlt_D_B(
+  const math::LinearSolver<Eigen::LDLT, MatrixUpTo6<T>>& get_ldlt_D_B(
       BodyNodeIndex body_node_index) const {
     DRAKE_ASSERT(0 <= body_node_index && body_node_index < num_nodes_);
     return ldlt_D_B_[body_node_index];
   }
 
   // Mutable version of get_ldlt_D_B().
-  Eigen::LDLT<MatrixUpTo6<T>>& get_mutable_ldlt_D_B(
+  math::LinearSolver<Eigen::LDLT, MatrixUpTo6<T>>& get_mutable_ldlt_D_B(
       BodyNodeIndex body_node_index) {
     DRAKE_ASSERT(0 <= body_node_index && body_node_index < num_nodes_);
     return ldlt_D_B_[body_node_index];
@@ -127,7 +128,7 @@ class ArticulatedBodyInertiaCache {
   // Pools indexed by BodyNodeIndex.
   std::vector<ArticulatedBodyInertia<T>> P_B_W_;
   std::vector<ArticulatedBodyInertia<T>> Pplus_PB_W_;
-  std::vector<Eigen::LDLT<MatrixUpTo6<T>>> ldlt_D_B_;
+  std::vector<math::LinearSolver<Eigen::LDLT, MatrixUpTo6<T>>> ldlt_D_B_;
   std::vector<Matrix6xUpTo6<T>> g_PB_W_;
 };
 

multibody/tree/body_node.h

@@ -1059,15 +1059,17 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
 
       // Compute the LDLT factorization of D_B as ldlt_D_B.
       // TODO(bobbyluig): Test performance against inverse().
-      Eigen::LDLT<MatrixUpTo6<T>>& ldlt_D_B = get_mutable_ldlt_D_B(abic);
-      ldlt_D_B = D_B.template selfadjointView<Eigen::Lower>().ldlt();
+      math::LinearSolver<Eigen::LDLT, MatrixUpTo6<T>>& ldlt_D_B =
+          get_mutable_ldlt_D_B(abic);
+      ldlt_D_B = math::LinearSolver<Eigen::LDLT, MatrixUpTo6<T>>(
+          MatrixUpTo6<T>(D_B.template selfadjointView<Eigen::Lower>()));
 
       // Ensure that D_B is not singular.
       // Singularity means that a non-physical hinge mapping matrix was used or
       // that this articulated body inertia has some non-physical quantities
       // (such as zero moment of inertia along an axis which the hinge mapping
       // matrix permits motion).
-      if (ldlt_D_B.info() != Eigen::Success) {
+      if (ldlt_D_B.eigen_linear_solver().info() != Eigen::Success) {
         std::stringstream message;
         message << "Encountered singular articulated body hinge inertia "
                 << "for body node index " << topology_.index << ". "
@@ -1078,7 +1080,7 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
 
       // Compute the Kalman gain, g_PB_W, using (6).
       Matrix6xUpTo6<T>& g_PB_W = get_mutable_g_PB_W(abic);
-      g_PB_W = ldlt_D_B.solve(U_B_W).transpose();
+      g_PB_W = ldlt_D_B.Solve(U_B_W).transpose();
 
       // Project P_B_W using (7) to obtain Pplus_PB_W, the articulated body
       // inertia of this body B as felt by body P and expressed in frame W.
@@ -1264,7 +1266,7 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
       // Compute nu_B, the articulated body inertia innovations generalized
       // acceleration.
       const VectorUpTo6<T> nu_B =
-          get_ldlt_D_B(abic).solve(get_e_B(aba_force_cache));
+          get_ldlt_D_B(abic).Solve(get_e_B(aba_force_cache));
 
       // Mutable reference to the generalized acceleration.
       auto vmdot = get_mutable_accelerations(ac);
@@ -1640,13 +1642,13 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
 
   // Returns a const reference to the LDLT factorization `ldlt_D_B` of the
   // articulated body hinge inertia.
-  const Eigen::LDLT<MatrixUpTo6<T>>& get_ldlt_D_B(
+  const math::LinearSolver<Eigen::LDLT, MatrixUpTo6<T>>& get_ldlt_D_B(
       const ArticulatedBodyInertiaCache<T>& abic) const {
     return abic.get_ldlt_D_B(topology_.index);
   }
 
   // Mutable version of get_ldlt_D_B().
-  Eigen::LDLT<MatrixUpTo6<T>>& get_mutable_ldlt_D_B(
+  math::LinearSolver<Eigen::LDLT, MatrixUpTo6<T>>& get_mutable_ldlt_D_B(
       ArticulatedBodyInertiaCache<T>* abic) const {
     return abic->get_mutable_ldlt_D_B(topology_.index);
   }