sleipnirgroup-jormungandr 0.5.6.dev4__tar.gz → 0.5.6.dev6__tar.gz

{sleipnirgroup_jormungandr-0.5.6.dev4 → sleipnirgroup_jormungandr-0.5.6.dev6}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: sleipnirgroup-jormungandr
-Version: 0.5.6.dev4
+Version: 0.5.6.dev6
 Summary: Reverse mode autodiff library and NLP solver DSL
 License-Expression: BSD-3-Clause
 License-File: LICENSE.txt

{sleipnirgroup_jormungandr-0.5.6.dev4 → sleipnirgroup_jormungandr-0.5.6.dev6}/include/sleipnir/optimization/solver/interior_point.hpp

@@ -333,10 +333,20 @@ ExitStatus interior_point(
   // Kept outside the loop so its storage can be reused
   gch::small_vector<Eigen::Triplet<Scalar>> triplets;
 
-  // Constraint regularization is forced to zero in feasibility restoration
-  // because the equality constraint Jacobian cannot be rank-deficient
+  const int lhs_rows =
+      matrices.num_decision_variables + matrices.num_equality_constraints;
   RegularizedLDLT<Scalar> solver{
+      // Use sparse solver if lower triangle fills < 25% of system
+      H.nonZeros() +
+              (A_i.transpose() * A_i)
+                  .template triangularView<Eigen::Lower>()
+                  .eval()
+                  .nonZeros() +
+              A_e.nonZeros() <
+          0.25 * lhs_rows * lhs_rows,
       matrices.num_decision_variables, matrices.num_equality_constraints,
+      // Constraint regularization is forced to zero in feasibility restoration
+      // because the equality constraint Jacobian cannot be rank-deficient
       in_feasibility_restoration ? Scalar(0) : Scalar(1e-10)};
 
   // Variables for determining when a step is acceptable

{sleipnirgroup_jormungandr-0.5.6.dev4 → sleipnirgroup_jormungandr-0.5.6.dev6}/include/sleipnir/optimization/solver/newton.hpp

@@ -132,7 +132,9 @@ ExitStatus newton(
 
   Filter<Scalar> filter;
 
-  RegularizedLDLT<Scalar> solver{matrices.num_decision_variables, 0};
+  RegularizedLDLT<Scalar> solver{
+      // Use sparse solver if lower triangle fills < 25% of system
+      H.nonZeros() < 0.25 * H.size(), matrices.num_decision_variables, 0};
 
   // Variables for determining when a step is acceptable
   constexpr Scalar α_reduction_factor(0.5);

{sleipnirgroup_jormungandr-0.5.6.dev4 → sleipnirgroup_jormungandr-0.5.6.dev6}/include/sleipnir/optimization/solver/sqp.hpp

@@ -225,8 +225,12 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
   // Kept outside the loop so its storage can be reused
   gch::small_vector<Eigen::Triplet<Scalar>> triplets;
 
-  RegularizedLDLT<Scalar> solver{matrices.num_decision_variables,
-                                 matrices.num_equality_constraints};
+  const int lhs_rows =
+      matrices.num_decision_variables + matrices.num_equality_constraints;
+  RegularizedLDLT<Scalar> solver{
+      // Use sparse solver if lower triangle fills < 25% of system
+      H.nonZeros() + A_e.nonZeros() < 0.25 * lhs_rows * lhs_rows,
+      matrices.num_decision_variables, matrices.num_equality_constraints};
 
   // Variables for determining when a step is acceptable
   constexpr Scalar α_reduction_factor(0.5);

{sleipnirgroup_jormungandr-0.5.6.dev4 → sleipnirgroup_jormungandr-0.5.6.dev6}/include/sleipnir/optimization/solver/util/all_finite.hpp

@@ -6,6 +6,8 @@
 
 #include <Eigen/SparseCore>
 
+namespace slp {
+
 /// Returns true if elements of sparse matrix are all finite.
 ///
 /// @param mat Sparse matrix.
@@ -24,3 +26,5 @@ bool all_finite(const Eigen::SparseCompressedBase<Derived>& mat) {
 
   return true;
 }
+
+}  // namespace slp

{sleipnirgroup_jormungandr-0.5.6.dev4 → sleipnirgroup_jormungandr-0.5.6.dev6}/include/sleipnir/optimization/solver/util/append_as_triplets.hpp

@@ -7,6 +7,8 @@
 #include <Eigen/SparseCore>
 #include <gch/small_vector.hpp>
 
+namespace slp {
+
 /// Appends sparse matrices to list of triplets at the given offset.
 ///
 /// The triplets are appended in column-major order (e.g., first column of mat1,
@@ -58,3 +60,5 @@ void append_diagonal_as_triplets(
     triplets.emplace_back(row_offset + row, col_offset + row, diag[row]);
   }
 }
+
+}  // namespace slp

sleipnirgroup_jormungandr-0.5.6.dev6/include/sleipnir/optimization/solver/util/dense_regularized_ldlt.hpp

@@ -0,0 +1,186 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <Eigen/Cholesky>
+#include <Eigen/Core>
+
+#include "sleipnir/optimization/solver/util/inertia.hpp"
+
+// See docs/algorithms.md#Works_cited for citation definitions
+
+namespace slp {
+
+/// Solves dense systems of linear equations using a regularized LDLT
+/// factorization.
+///
+/// @tparam Scalar Scalar type.
+template <typename Scalar>
+class DenseRegularizedLDLT {
+ public:
+  /// Type alias for dense matrix.
+  using DenseMatrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+  /// Type alias for dense vector.
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+
+  /// Constructs a DenseRegularizedLDLT instance.
+  ///
+  /// @param num_decision_variables The number of decision variables in the
+  ///     system.
+  /// @param num_equality_constraints The number of equality constraints in the
+  ///     system.
+  DenseRegularizedLDLT(int num_decision_variables, int num_equality_constraints)
+      : m_num_decision_variables{num_decision_variables},
+        m_num_equality_constraints{num_equality_constraints} {}
+
+  /// Constructs a DenseRegularizedLDLT instance.
+  ///
+  /// @param num_decision_variables The number of decision variables in the
+  ///     system.
+  /// @param num_equality_constraints The number of equality constraints in the
+  ///     system.
+  /// @param γ_min The minimum constraint regularization.
+  DenseRegularizedLDLT(int num_decision_variables, int num_equality_constraints,
+                       Scalar γ_min)
+      : m_num_decision_variables{num_decision_variables},
+        m_num_equality_constraints{num_equality_constraints},
+        m_γ_min{γ_min} {}
+
+  /// Reports whether previous computation was successful.
+  ///
+  /// @return Whether previous computation was successful.
+  Eigen::ComputationInfo info() const { return m_info; }
+
+  /// Computes the regularized LDLT factorization of a matrix.
+  ///
+  /// @param lhs Left-hand side of the system.
+  /// @return The factorization.
+  DenseRegularizedLDLT& compute(const DenseMatrix& lhs) {
+    // The regularization procedure is based on algorithm B.1 of [1]
+
+    m_info = m_solver.compute(lhs).info();
+
+    if (m_info == Eigen::Success) {
+      auto D = m_solver.vectorD();
+
+      // If the inertia is ideal and D from LDLT is sufficiently far from zero,
+      // don't regularize the system
+      if (Inertia{D} == ideal_inertia &&
+          (D.cwiseAbs().array() >= Scalar(1e-4)).all()) {
+        m_prev_δ = Scalar(0);
+        return *this;
+      }
+    }
+
+    // Also regularize the Hessian. If the Hessian wasn't regularized in a
+    // previous run of compute(), start at small values of δ and γ. Otherwise,
+    // attempt a δ and γ half as big as the previous run so δ and γ can trend
+    // downwards over time.
+    Scalar δ = m_prev_δ == Scalar(0) ? Scalar(1e-4) : m_prev_δ / Scalar(2);
+    Scalar γ = m_γ_min;
+
+    while (true) {
+      m_info = m_solver.compute(lhs + regularization(δ, γ)).info();
+
+      if (m_info == Eigen::Success) {
+        Inertia inertia{m_solver.vectorD()};
+
+        if (inertia == ideal_inertia) {
+          // If the inertia is ideal, store δ and return
+          m_prev_δ = δ;
+          return *this;
+        } else if (inertia.zero > 0) {
+          // If there's zero eigenvalues, increase δ and γ by an order of
+          // magnitude and try again
+          δ *= Scalar(10);
+          γ *= Scalar(10);
+        } else if (inertia.negative > ideal_inertia.negative) {
+          // If there's too many negative eigenvalues, increase δ by an order of
+          // magnitude and try again
+          δ *= Scalar(10);
+        } else if (inertia.positive > ideal_inertia.positive) {
+          // If there's too many positive eigenvalues, increase γ by an order of
+          // magnitude and try again
+          γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10);
+        }
+      } else {
+        // If the decomposition failed, increase δ and γ by an order of
+        // magnitude and try again
+        δ *= Scalar(10);
+        γ *= Scalar(10);
+      }
+
+      // If the Hessian perturbation is too high, report failure. This can be
+      // caused by ill-conditioning.
+      if (δ > Scalar(1e20) || γ > Scalar(1e20)) {
+        m_info = Eigen::NumericalIssue;
+        return *this;
+      }
+    }
+  }
+
+  /// Solves the system of equations using a regularized LDLT factorization.
+  ///
+  /// @param rhs Right-hand side of the system.
+  /// @return The solution.
+  template <typename Rhs>
+  DenseVector solve(const Eigen::MatrixBase<Rhs>& rhs) const {
+    return m_solver.solve(rhs);
+  }
+
+  /// Solves the system of equations using a regularized LDLT factorization.
+  ///
+  /// @param rhs Right-hand side of the system.
+  /// @return The solution.
+  template <typename Rhs>
+  DenseVector solve(const Eigen::SparseMatrixBase<Rhs>& rhs) const {
+    return m_solver.solve(rhs.toDense());
+  }
+
+  /// Returns the Hessian regularization factor.
+  ///
+  /// @return Hessian regularization factor.
+  Scalar hessian_regularization() const { return m_prev_δ; }
+
+ private:
+  using Solver = Eigen::LDLT<DenseMatrix>;
+
+  Solver m_solver;
+
+  Eigen::ComputationInfo m_info = Eigen::Success;
+
+  /// The number of decision variables in the system.
+  int m_num_decision_variables = 0;
+
+  /// The number of equality constraints in the system.
+  int m_num_equality_constraints = 0;
+
+  /// The minimum constraint regularization.
+  Scalar m_γ_min{1e-10};
+
+  /// The ideal system inertia.
+  Inertia ideal_inertia{m_num_decision_variables, m_num_equality_constraints,
+                        0};
+
+  /// The value of δ from the previous run of compute().
+  Scalar m_prev_δ{0};
+
+  /// Returns regularization matrix.
+  ///
+  ///   [δI    0]
+  ///   [ 0  −γI]
+  ///
+  /// @param δ The Hessian regularization factor.
+  /// @param γ The equality constraint Jacobian regularization factor.
+  /// @return Regularization matrix.
+  DenseMatrix regularization(Scalar δ, Scalar γ) const {
+    DenseVector vec{m_num_decision_variables + m_num_equality_constraints};
+    vec.segment(0, m_num_decision_variables).setConstant(δ);
+    vec.segment(m_num_decision_variables, m_num_equality_constraints)
+        .setConstant(-γ);
+
+    return vec.asDiagonal().toDenseMatrix();
+  }
+};
+
+}  // namespace slp

sleipnirgroup_jormungandr-0.5.6.dev6/include/sleipnir/optimization/solver/util/regularized_ldlt.hpp

@@ -0,0 +1,125 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
+
+#include "sleipnir/optimization/solver/util/dense_regularized_ldlt.hpp"
+#include "sleipnir/optimization/solver/util/sparse_regularized_ldlt.hpp"
+
+namespace slp {
+
+/// Solves systems of linear equations using a regularized LDLT factorization.
+///
+/// @tparam Scalar Scalar type.
+template <typename Scalar>
+class RegularizedLDLT {
+ public:
+  /// Type alias for dense vector.
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+  /// Type alias for sparse matrix.
+  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
+
+  /// Constructs a RegularizedLDLT instance.
+  ///
+  /// @param use_sparse_solver Whether to use sparse or dense solver.
+  /// @param num_decision_variables The number of decision variables in the
+  ///     system.
+  /// @param num_equality_constraints The number of equality constraints in the
+  ///     system.
+  RegularizedLDLT(bool use_sparse_solver, int num_decision_variables,
+                  int num_equality_constraints)
+      : m_use_sparse_solver{use_sparse_solver},
+        m_sparse_solver{num_decision_variables, num_equality_constraints},
+        m_dense_solver{num_decision_variables, num_equality_constraints} {}
+
+  /// Constructs a RegularizedLDLT instance.
+  ///
+  /// @param use_sparse_solver Whether to use sparse or dense solver.
+  /// @param num_decision_variables The number of decision variables in the
+  ///     system.
+  /// @param num_equality_constraints The number of equality constraints in the
+  ///     system.
+  /// @param γ_min The minimum constraint regularization.
+  RegularizedLDLT(bool use_sparse_solver, int num_decision_variables,
+                  int num_equality_constraints, Scalar γ_min)
+      : m_use_sparse_solver{use_sparse_solver},
+        m_sparse_solver{num_decision_variables, num_equality_constraints,
+                        γ_min},
+        m_dense_solver{num_decision_variables, num_equality_constraints,
+                       γ_min} {}
+
+  /// Reports whether previous computation was successful.
+  ///
+  /// @return Whether previous computation was successful.
+  Eigen::ComputationInfo info() const {
+    if (m_use_sparse_solver) {
+      return m_sparse_solver.info();
+    } else {
+      return m_dense_solver.info();
+    }
+  }
+
+  /// Computes the regularized LDLT factorization of a matrix.
+  ///
+  /// In sparse mode, the matrix's symbolic decomposition is reused in
+  /// subsequent calls, so subsequent calls must be given a matrix with the same
+  /// sparsity pattern.
+  ///
+  /// @param lhs Left-hand side of the system.
+  /// @return The factorization.
+  RegularizedLDLT& compute(const SparseMatrix& lhs) {
+    if (m_use_sparse_solver) {
+      m_sparse_solver.compute(lhs);
+    } else {
+      m_dense_solver.compute(lhs);
+    }
+
+    return *this;
+  }
+
+  /// Solves the system of equations using a regularized LDLT factorization.
+  ///
+  /// @param rhs Right-hand side of the system.
+  /// @return The solution.
+  template <typename Rhs>
+  DenseVector solve(const Eigen::MatrixBase<Rhs>& rhs) const {
+    if (m_use_sparse_solver) {
+      return m_sparse_solver.solve(rhs);
+    } else {
+      return m_dense_solver.solve(rhs);
+    }
+  }
+
+  /// Solves the system of equations using a regularized LDLT factorization.
+  ///
+  /// @param rhs Right-hand side of the system.
+  /// @return The solution.
+  template <typename Rhs>
+  DenseVector solve(const Eigen::SparseMatrixBase<Rhs>& rhs) const {
+    if (m_use_sparse_solver) {
+      return m_sparse_solver.solve(rhs);
+    } else {
+      return m_dense_solver.solve(rhs);
+    }
+  }
+
+  /// Returns the Hessian regularization factor.
+  ///
+  /// @return Hessian regularization factor.
+  Scalar hessian_regularization() const {
+    if (m_use_sparse_solver) {
+      return m_sparse_solver.hessian_regularization();
+    } else {
+      return m_dense_solver.hessian_regularization();
+    }
+  }
+
+ private:
+  bool m_use_sparse_solver;
+  SparseRegularizedLDLT<Scalar> m_sparse_solver;
+  DenseRegularizedLDLT<Scalar> m_dense_solver;
+};
+
+}  // namespace slp

sleipnirgroup_jormungandr-0.5.6.dev4/include/sleipnir/optimization/solver/util/regularized_ldlt.hpp → sleipnirgroup_jormungandr-0.5.6.dev6/include/sleipnir/optimization/solver/util/sparse_regularized_ldlt.hpp

@@ -2,7 +2,6 @@
 
 #pragma once
 
-#include <Eigen/Cholesky>
 #include <Eigen/Core>
 #include <Eigen/SparseCholesky>
 #include <Eigen/SparseCore>
@@ -13,38 +12,38 @@
 
 namespace slp {
 
-/// Solves systems of linear equations using a regularized LDLT factorization.
+/// Solves sparse systems of linear equations using a regularized LDLT
+/// factorization.
 ///
 /// @tparam Scalar Scalar type.
 template <typename Scalar>
-class RegularizedLDLT {
+class SparseRegularizedLDLT {
  public:
-  /// Type alias for dense matrix.
-  using DenseMatrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
   /// Type alias for dense vector.
   using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
   /// Type alias for sparse matrix.
   using SparseMatrix = Eigen::SparseMatrix<Scalar>;
 
-  /// Constructs a RegularizedLDLT instance.
+  /// Constructs a SparseRegularizedLDLT instance.
   ///
   /// @param num_decision_variables The number of decision variables in the
   ///     system.
   /// @param num_equality_constraints The number of equality constraints in the
   ///     system.
-  RegularizedLDLT(int num_decision_variables, int num_equality_constraints)
+  SparseRegularizedLDLT(int num_decision_variables,
+                        int num_equality_constraints)
       : m_num_decision_variables{num_decision_variables},
         m_num_equality_constraints{num_equality_constraints} {}
 
-  /// Constructs a RegularizedLDLT instance.
+  /// Constructs a SparseRegularizedLDLT instance.
   ///
   /// @param num_decision_variables The number of decision variables in the
   ///     system.
   /// @param num_equality_constraints The number of equality constraints in the
   ///     system.
   /// @param γ_min The minimum constraint regularization.
-  RegularizedLDLT(int num_decision_variables, int num_equality_constraints,
-                  Scalar γ_min)
+  SparseRegularizedLDLT(int num_decision_variables,
+                        int num_equality_constraints, Scalar γ_min)
       : m_num_decision_variables{num_decision_variables},
         m_num_equality_constraints{num_equality_constraints},
         m_γ_min{γ_min} {}
@@ -61,23 +60,23 @@ class RegularizedLDLT {
   ///
   /// @param lhs Left-hand side of the system.
   /// @return The factorization.
-  RegularizedLDLT& compute(const SparseMatrix& lhs) {
+  SparseRegularizedLDLT& compute(const SparseMatrix& lhs) {
     // The regularization procedure is based on algorithm B.1 of [1]
 
-    // Max density is 50% due to the caller only providing the lower triangle.
-    // We consider less than 25% to be sparse.
-    m_is_sparse = lhs.nonZeros() < 0.25 * lhs.size();
+    // Regularization with zeros ensures the pattern analysis in the sparse
+    // solver is reused by all factorizations
+    SparseMatrix unregularized_lhs = lhs + regularization(Scalar(0), Scalar(0));
 
-    // Regularization with zeros ensures the pattern analysis in
-    // compute_sparse() is reused by all factorizations
-    m_info =
-        m_is_sparse
-            ? compute_sparse(lhs + regularization(Scalar(0), Scalar(0))).info()
-            : m_dense_solver.compute(lhs).info();
+    if (!m_analyzed_pattern) {
+      m_solver.analyzePattern(unregularized_lhs);
+      m_analyzed_pattern = true;
+    }
+
+    m_solver.factorize(unregularized_lhs);
+    m_info = m_solver.info();
 
     if (m_info == Eigen::Success) {
-      auto D =
-          m_is_sparse ? m_sparse_solver.vectorD() : m_dense_solver.vectorD();
+      auto D = m_solver.vectorD();
 
       // If the inertia is ideal and D from LDLT is sufficiently far from zero,
       // don't regularize the system
@@ -96,25 +95,12 @@ class RegularizedLDLT {
     Scalar γ = m_γ_min;
 
     while (true) {
-      Inertia inertia;
-
-      // Regularize lhs by adding a multiple of the identity matrix
-      //
-      // lhs = [H + AᵢᵀΣAᵢ + δI  Aₑᵀ]
-      //       [      Aₑ         −γI]
-      if (m_is_sparse) {
-        m_info = compute_sparse(lhs + regularization(δ, γ)).info();
-        if (m_info == Eigen::Success) {
-          inertia = Inertia{m_sparse_solver.vectorD()};
-        }
-      } else {
-        m_info = m_dense_solver.compute(lhs + regularization(δ, γ)).info();
-        if (m_info == Eigen::Success) {
-          inertia = Inertia{m_dense_solver.vectorD()};
-        }
-      }
+      m_solver.factorize(lhs + regularization(δ, γ));
+      m_info = m_solver.info();
 
       if (m_info == Eigen::Success) {
+        Inertia inertia{m_solver.vectorD()};
+
         if (inertia == ideal_inertia) {
           // If the inertia is ideal, store δ and return
           m_prev_δ = δ;
@@ -155,11 +141,7 @@ class RegularizedLDLT {
   /// @return The solution.
   template <typename Rhs>
   DenseVector solve(const Eigen::MatrixBase<Rhs>& rhs) const {
-    if (m_is_sparse) {
-      return m_sparse_solver.solve(rhs);
-    } else {
-      return m_dense_solver.solve(rhs);
-    }
+    return m_solver.solve(rhs);
   }
 
   /// Solves the system of equations using a regularized LDLT factorization.
@@ -168,11 +150,7 @@ class RegularizedLDLT {
   /// @return The solution.
   template <typename Rhs>
   DenseVector solve(const Eigen::SparseMatrixBase<Rhs>& rhs) const {
-    if (m_is_sparse) {
-      return m_sparse_solver.solve(rhs);
-    } else {
-      return m_dense_solver.solve(rhs.toDense());
-    }
+    return m_solver.solve(rhs);
   }
 
   /// Returns the Hessian regularization factor.
@@ -181,12 +159,10 @@ class RegularizedLDLT {
   Scalar hessian_regularization() const { return m_prev_δ; }
 
  private:
-  using SparseSolver = Eigen::SimplicialLDLT<SparseMatrix>;
-  using DenseSolver = Eigen::LDLT<DenseMatrix>;
+  using Solver = Eigen::SimplicialLDLT<SparseMatrix>;
 
-  SparseSolver m_sparse_solver;
-  DenseSolver m_dense_solver;
-  bool m_is_sparse = true;
+  Solver m_solver;
+  bool m_analyzed_pattern = false;
 
   Eigen::ComputationInfo m_info = Eigen::Success;
 
@@ -206,28 +182,11 @@ class RegularizedLDLT {
   /// The value of δ from the previous run of compute().
   Scalar m_prev_δ{0};
 
-  // Number of non-zeros in LHS.
-  int m_non_zeros = -1;
-
-  /// Computes factorization of a sparse matrix.
-  ///
-  /// @param lhs Matrix to factorize.
-  /// @return The factorization.
-  SparseSolver& compute_sparse(const SparseMatrix& lhs) {
-    // Reanalize lhs's sparsity pattern if it changed
-    int non_zeros = lhs.nonZeros();
-    if (m_non_zeros != non_zeros) {
-      m_sparse_solver.analyzePattern(lhs);
-      m_non_zeros = non_zeros;
-    }
-
-    m_sparse_solver.factorize(lhs);
-
-    return m_sparse_solver;
-  }
-
   /// Returns regularization matrix.
   ///
+  ///   [δI    0]
+  ///   [ 0  −γI]
+  ///
   /// @param δ The Hessian regularization factor.
   /// @param γ The equality constraint Jacobian regularization factor.
   /// @return Regularization matrix.

{sleipnirgroup_jormungandr-0.5.6.dev4 → sleipnirgroup_jormungandr-0.5.6.dev6}/python/src/sleipnir/__init__.py

@@ -2,4 +2,4 @@
 
 from ._sleipnir import *
 
-__version__ = "0.5.6.dev4"
+__version__ = "0.5.6.dev6"