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

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

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

{sleipnirgroup_jormungandr-0.5.6.dev6 → sleipnirgroup_jormungandr-0.5.6.dev8}/include/sleipnir/optimization/problem.hpp

@@ -30,6 +30,7 @@
 #include "sleipnir/optimization/solver/options.hpp"
 #include "sleipnir/optimization/solver/sqp.hpp"
 #include "sleipnir/optimization/solver/util/bounds.hpp"
+#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
 #include "sleipnir/util/empty.hpp"
 #include "sleipnir/util/print.hpp"
 #include "sleipnir/util/print_diagnostics.hpp"
@@ -375,20 +376,26 @@ class Problem {
       }
 #endif
 
+      // Automatically scale the cost. The problem scaling procedure is
+      // described in more detail in docs/algorithms.md#problem-scaling.
+      x_ad.set_value(x);
+      const ProblemScaling<Scalar> scaling{g.value()};
+
       NewtonMatrixCallbacks<Scalar> matrix_callbacks{
           num_decision_variables,
           [&](const DenseVector& x) -> Scalar {
             x_ad.set_value(x);
-            return f.value();
+            return scaling.f * f.value();
           },
           [&](const DenseVector& x) -> SparseVector {
             x_ad.set_value(x);
-            return g.value();
+            return scaling.f * g.value();
           },
           [&](const DenseVector& x) -> SparseMatrix {
             x_ad.set_value(x);
-            return H.value();
-          }};
+            return scaling.f * H.value();
+          },
+          scaling};
 
       // Invoke Newton solver
       status =
@@ -463,35 +470,42 @@ class Problem {
       }
 #endif
 
+      // Automatically scale the cost and constraints. The problem scaling
+      // procedure is described in more detail in
+      // docs/algorithms.md#problem-scaling.
+      x_ad.set_value(x);
+      const ProblemScaling<Scalar> scaling{g.value(), A_e.value()};
+
       SQPMatrixCallbacks<Scalar> matrix_callbacks{
           num_decision_variables,
           num_equality_constraints,
           [&](const DenseVector& x) -> Scalar {
             x_ad.set_value(x);
-            return f.value();
+            return scaling.f * f.value();
           },
           [&](const DenseVector& x) -> SparseVector {
             x_ad.set_value(x);
-            return g.value();
+            return scaling.f * g.value();
           },
           [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix {
             x_ad.set_value(x);
-            y_ad.set_value(y);
-            return H_f.value() + H_c.value();
+            y_ad.set_value(scaling.c_e.cwiseProduct(y));
+            return scaling.f * H_f.value() + H_c.value();
           },
           [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix {
             x_ad.set_value(x);
-            y_ad.set_value(y);
+            y_ad.set_value(scaling.c_e.cwiseProduct(y));
             return H_c.value();
           },
           [&](const DenseVector& x) -> DenseVector {
             x_ad.set_value(x);
-            return c_e_ad.value();
+            return scaling.c_e.cwiseProduct(c_e_ad.value());
           },
           [&](const DenseVector& x) -> SparseMatrix {
             x_ad.set_value(x);
-            return A_e.value();
-          }};
+            return scaling.c_e.asDiagonal() * A_e.value();
+          },
+          scaling};
 
       // Invoke SQP solver
       status = sqp<Scalar>(matrix_callbacks, iteration_callbacks, options, x);
@@ -595,48 +609,55 @@ class Problem {
       project_onto_bounds(x, bounds);
 #endif
 
+      // Automatically scale the cost and constraints. The problem scaling
+      // procedure is described in more detail in
+      // docs/algorithms.md#problem-scaling.
+      x_ad.set_value(x);
+      const ProblemScaling<Scalar> scaling{g.value(), A_e.value(), A_i.value()};
+
       InteriorPointMatrixCallbacks<Scalar> matrix_callbacks{
           num_decision_variables,
           num_equality_constraints,
           num_inequality_constraints,
           [&](const DenseVector& x) -> Scalar {
             x_ad.set_value(x);
-            return f.value();
+            return scaling.f * f.value();
           },
           [&](const DenseVector& x) -> SparseVector {
             x_ad.set_value(x);
-            return g.value();
+            return scaling.f * g.value();
           },
           [&](const DenseVector& x, const DenseVector& y,
               const DenseVector& z) -> SparseMatrix {
             x_ad.set_value(x);
-            y_ad.set_value(y);
-            z_ad.set_value(z);
-            return H_f.value() + H_c.value();
+            y_ad.set_value(scaling.c_e.cwiseProduct(y));
+            z_ad.set_value(scaling.c_i.cwiseProduct(z));
+            return scaling.f * H_f.value() + H_c.value();
           },
           [&](const DenseVector& x, const DenseVector& y,
               const DenseVector& z) -> SparseMatrix {
             x_ad.set_value(x);
-            y_ad.set_value(y);
-            z_ad.set_value(z);
+            y_ad.set_value(scaling.c_e.cwiseProduct(y));
+            z_ad.set_value(scaling.c_i.cwiseProduct(z));
             return H_c.value();
           },
           [&](const DenseVector& x) -> DenseVector {
             x_ad.set_value(x);
-            return c_e_ad.value();
+            return scaling.c_e.cwiseProduct(c_e_ad.value());
           },
           [&](const DenseVector& x) -> SparseMatrix {
             x_ad.set_value(x);
-            return A_e.value();
+            return scaling.c_e.asDiagonal() * A_e.value();
           },
           [&](const DenseVector& x) -> DenseVector {
             x_ad.set_value(x);
-            return c_i_ad.value();
+            return scaling.c_i.cwiseProduct(c_i_ad.value());
           },
           [&](const DenseVector& x) -> SparseMatrix {
             x_ad.set_value(x);
-            return A_i.value();
-          }};
+            return scaling.c_i.asDiagonal() * A_i.value();
+          },
+          scaling};
 
       // Invoke interior-point method solver
       status =

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

@@ -76,7 +76,7 @@ ExitStatus interior_point(
   DenseVector y = DenseVector::Zero(matrix_callbacks.num_equality_constraints);
   DenseVector z =
       DenseVector::Ones(matrix_callbacks.num_inequality_constraints);
-  Scalar μ(0.1);
+  Scalar μ = Scalar(0.1) * matrix_callbacks.scaling.f;
 
   return interior_point(matrix_callbacks, iteration_callbacks, options, false,
 #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
@@ -231,7 +231,8 @@ ExitStatus interior_point(
       [&](const DenseVector& x) -> SparseMatrix {
         ScopedProfiler prof{A_i_prof};
         return matrix_callbacks.A_i(x);
-      }};
+      },
+      matrix_callbacks.scaling};
 #else
   const auto& matrices = matrix_callbacks;
 #endif
@@ -290,7 +291,8 @@ ExitStatus interior_point(
   int iterations = 0;
 
   // Barrier parameter minimum
-  const Scalar μ_min = Scalar(options.tolerance) / Scalar(10);
+  const Scalar μ_min =
+      matrices.scaling.f * Scalar(options.tolerance) / Scalar(10);
 
   // Fraction-to-the-boundary rule scale factor minimum
   constexpr Scalar τ_min(0.99);
@@ -356,8 +358,8 @@ ExitStatus interior_point(
   int full_step_rejected_counter = 0;
 
   // Error
-  Scalar E_0 = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
-      g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0));
+  Scalar E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
+      matrices.scaling, g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0));
 
   setup_prof.stop();
 
@@ -569,9 +571,9 @@ ExitStatus interior_point(
                   step_acceptable ? IterationType::ACCEPTED_SOC
                                   : IterationType::REJECTED_SOC,
                   soc_profiler.current_duration(),
-                  kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
-                      g, A_e, trial_c_e, A_i, trial_c_i, trial_s, trial_y,
-                      trial_z, Scalar(0)),
+                  unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
+                      matrices.scaling, g, A_e, trial_c_e, A_i, trial_c_i,
+                      trial_s, trial_y, trial_z, Scalar(0)),
                   trial_f,
                   trial_c_e.template lpNorm<1>() +
                       (trial_c_i - trial_s).template lpNorm<1>(),
@@ -781,8 +783,8 @@ ExitStatus interior_point(
     H = matrices.H(x, y, z);
 
     // Update the error
-    E_0 = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
-        g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0));
+    E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
+        matrices.scaling, g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0));
 
     // Update the barrier parameter if necessary
     if (E_0 > Scalar(options.tolerance)) {

{sleipnirgroup_jormungandr-0.5.6.dev6 → sleipnirgroup_jormungandr-0.5.6.dev8}/include/sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp

@@ -7,6 +7,8 @@
 #include <Eigen/Core>
 #include <Eigen/SparseCore>
 
+#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
+
 namespace slp {
 
 /// Matrix callbacks for the interior-point method solver.
@@ -241,6 +243,10 @@ struct InteriorPointMatrixCallbacks {
   ///   </tr>
   /// </table>
   std::function<SparseMatrix(const DenseVector& x)> A_i;
+
+  /// Automatic problem scaling factors. Used to scale the cost, constraints,
+  /// and tolerance inside the interior-point solver.
+  ProblemScaling<Scalar> scaling;
 };
 
 }  // namespace slp

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

@@ -102,7 +102,8 @@ ExitStatus newton(
       [&](const DenseVector& x) -> SparseMatrix {
         ScopedProfiler prof{H_prof};
         return matrix_callbacks.H(x);
-      }};
+      },
+      matrix_callbacks.scaling};
 #else
   const auto& matrices = matrix_callbacks;
 #endif
@@ -141,7 +142,8 @@ ExitStatus newton(
   constexpr Scalar α_min(1e-20);
 
   // Error
-  Scalar E_0 = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(g);
+  Scalar E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
+      matrices.scaling, g);
 
   setup_prof.stop();
 
@@ -255,7 +257,8 @@ ExitStatus newton(
     H = matrices.H(x);
 
     // Update the error
-    E_0 = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(g);
+    E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
+        matrices.scaling, g);
 
     inner_iter_profiler.stop();
 

{sleipnirgroup_jormungandr-0.5.6.dev6 → sleipnirgroup_jormungandr-0.5.6.dev8}/include/sleipnir/optimization/solver/newton_matrix_callbacks.hpp

@@ -7,6 +7,8 @@
 #include <Eigen/Core>
 #include <Eigen/SparseCore>
 
+#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
+
 namespace slp {
 
 /// Matrix callbacks for the Newton's method solver.
@@ -90,6 +92,10 @@ struct NewtonMatrixCallbacks {
   ///   </tr>
   /// </table>
   std::function<SparseMatrix(const DenseVector& x)> H;
+
+  /// Automatic problem scaling factors. Used to scale the cost and tolerance
+  /// inside the Newton solver.
+  ProblemScaling<Scalar> scaling;
 };
 
 }  // namespace slp

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

@@ -175,7 +175,8 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
       [&](const DenseVector& x) -> SparseMatrix {
         ScopedProfiler prof{A_e_prof};
         return matrix_callbacks.A_e(x);
-      }};
+      },
+      matrix_callbacks.scaling};
 #else
   const auto& matrices = matrix_callbacks;
 #endif
@@ -239,7 +240,8 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
   int full_step_rejected_counter = 0;
 
   // Error
-  Scalar E_0 = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(g, A_e, c_e, y);
+  Scalar E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
+      matrices.scaling, g, A_e, c_e, y);
 
   setup_prof.stop();
 
@@ -555,7 +557,8 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
     H = matrices.H(x, y);
 
     // Update the error
-    E_0 = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(g, A_e, c_e, y);
+    E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
+        matrices.scaling, g, A_e, c_e, y);
 
     inner_iter_profiler.stop();
 

{sleipnirgroup_jormungandr-0.5.6.dev6 → sleipnirgroup_jormungandr-0.5.6.dev8}/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp

@@ -7,6 +7,8 @@
 #include <Eigen/Core>
 #include <Eigen/SparseCore>
 
+#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
+
 namespace slp {
 
 /// Matrix callbacks for the Sequential Quadratic Programming (SQP) solver.
@@ -175,6 +177,10 @@ struct SQPMatrixCallbacks {
   ///   </tr>
   /// </table>
   std::function<SparseMatrix(const DenseVector& x)> A_e;
+
+  /// Automatic problem scaling factors. Used to scale the cost, constraints,
+  /// and tolerance inside the SQP solver.
+  ProblemScaling<Scalar> scaling;
 };
 
 }  // namespace slp

{sleipnirgroup_jormungandr-0.5.6.dev6 → sleipnirgroup_jormungandr-0.5.6.dev8}/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp

@@ -169,6 +169,12 @@ ExitStatus feasibility_restoration(
 
   Scalar fr_μ = std::max(μ, c_e.template lpNorm<Eigen::Infinity>());
 
+  // Inherit the parent problem's scaling for the constraints, and use no
+  // scaling for the cost function since it has changed. The new rows introduced
+  // are not scaled.
+  const ProblemScaling<Scalar> fr_scaling{Scalar(1), matrices.scaling.c_e,
+                                          DenseVector::Ones(2 * num_eq)};
+
   InteriorPointMatrixCallbacks<Scalar> fr_matrix_callbacks{
       static_cast<int>(fr_x.rows()),
       static_cast<int>(fr_y.rows()),
@@ -287,7 +293,8 @@ ExitStatus feasibility_restoration(
         SparseMatrix A_i_p{2 * num_eq, x_p.rows()};
         A_i_p.setFromSortedTriplets(triplets.begin(), triplets.end());
         return A_i_p;
-      }};
+      },
+      fr_scaling};
 
   auto status = interior_point<Scalar>(fr_matrix_callbacks, iteration_callbacks,
                                        options, true,
@@ -401,6 +408,15 @@ ExitStatus feasibility_restoration(
   Scalar fr_μ = std::max({μ, c_e.template lpNorm<Eigen::Infinity>(),
                           (c_i - s).template lpNorm<Eigen::Infinity>()});
 
+  // Inherit the parent problem's scaling for the constraints, and use no
+  // scaling for the cost function since it has changed. The new rows introduced
+  // are not scaled.
+  DenseVector fr_d_c_i{c_i.rows() + 2 * num_eq + 2 * num_ineq};
+  fr_d_c_i << matrices.scaling.c_i,
+      DenseVector::Ones(2 * num_eq + 2 * num_ineq);
+  const ProblemScaling<Scalar> fr_scaling{Scalar(1), matrices.scaling.c_e,
+                                          fr_d_c_i};
+
   InteriorPointMatrixCallbacks<Scalar> fr_matrix_callbacks{
       static_cast<int>(fr_x.rows()),
       static_cast<int>(fr_y.rows()),
@@ -562,7 +578,8 @@ ExitStatus feasibility_restoration(
         SparseMatrix A_i_p{2 * num_eq + 3 * num_ineq, x_p.rows()};
         A_i_p.setFromSortedTriplets(triplets.begin(), triplets.end());
         return A_i_p;
-      }};
+      },
+      fr_scaling};
 
   auto status = interior_point<Scalar>(fr_matrix_callbacks, iteration_callbacks,
                                        options, true,

{sleipnirgroup_jormungandr-0.5.6.dev6 → sleipnirgroup_jormungandr-0.5.6.dev8}/include/sleipnir/optimization/solver/util/is_locally_infeasible.hpp

@@ -12,10 +12,8 @@ namespace slp {
 /// Returns true if the problem's equality constraints are locally infeasible.
 ///
 /// @tparam Scalar Scalar type.
-/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
-///     current iterate.
-/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
-///     iterate.
+/// @param A_e Equality constraint Jacobian Aₑ(x).
+/// @param c_e Equality constraints cₑ(x).
 template <typename Scalar>
 bool is_equality_locally_infeasible(
     const Eigen::SparseMatrix<Scalar>& A_e,
@@ -33,10 +31,8 @@ bool is_equality_locally_infeasible(
 /// Returns true if the problem's inequality constraints are locally infeasible.
 ///
 /// @tparam Scalar Scalar type.
-/// @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
-///     the current iterate.
-/// @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
-///     current iterate.
+/// @param A_i Inequality constraint Jacobian Aᵢ(x).
+/// @param c_i Inequality constraints cᵢ(x).
 template <typename Scalar>
 bool is_inequality_locally_infeasible(
     const Eigen::SparseMatrix<Scalar>& A_i,