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

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

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: sleipnirgroup-jormungandr
-Version: 0.5.6.dev6
+Version: 0.5.6.dev7
 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.dev7}/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.dev7}/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.dev7}/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.dev7}/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.dev7}/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.dev7}/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.dev7}/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.dev7}/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.dev7}/include/sleipnir/optimization/solver/util/kkt_error.hpp

@@ -7,6 +7,8 @@
 #include <Eigen/Core>
 #include <Eigen/SparseCore>
 
+#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
+
 // See docs/algorithms.md#Works_cited for citation definitions
 
 namespace slp {
@@ -149,4 +151,115 @@ Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
   }
 }
 
+/// Returns the unscaled KKT error for Newton's method.
+///
+/// @tparam Scalar Scalar type.
+/// @tparam T The type of KKT error to compute.
+/// @param scaling The problem scaling factors.
+/// @param g Gradient of the scaled cost function d_f·∇f.
+template <typename Scalar, KKTErrorType T>
+Scalar unscaled_kkt_error(const ProblemScaling<Scalar>& scaling,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& g) {
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+
+  if (scaling.is_identity()) {
+    return kkt_error<Scalar, T>(g);
+  }
+
+  const DenseVector g_unscaled = (Scalar(1) / scaling.f) * g;
+
+  return kkt_error<Scalar, T>(g_unscaled);
+}
+
+/// Returns the unscaled KKT error for Sequential Quadratic Programming.
+///
+/// @tparam Scalar Scalar type.
+/// @tparam T The type of KKT error to compute.
+/// @param scaling The problem scaling factors.
+/// @param g Gradient of the scaled cost function d_f·∇f.
+/// @param A_e The problem's scaled equality constraint Jacobian D_cₑ·Aₑ(x)
+///     evaluated at the current iterate.
+/// @param c_e The problem's scaled equality constraints D_cₑ·cₑ(x) evaluated
+///     at the current iterate.
+/// @param y Scaled equality constraint dual variables.
+template <typename Scalar, KKTErrorType T>
+Scalar unscaled_kkt_error(const ProblemScaling<Scalar>& scaling,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
+                          const Eigen::SparseMatrix<Scalar>& A_e,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& y) {
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
+
+  if (scaling.is_identity()) {
+    return kkt_error<Scalar, T>(g, A_e, c_e, y);
+  }
+
+  const Scalar inv_d_f = Scalar(1) / scaling.f;
+  const DenseVector inv_d_c_e = scaling.c_e.cwiseInverse();
+
+  const DenseVector g_unscaled = inv_d_f * g;
+  const SparseMatrix A_e_unscaled = inv_d_c_e.asDiagonal() * A_e;
+  const DenseVector c_e_unscaled = inv_d_c_e.cwiseProduct(c_e);
+  const DenseVector y_unscaled = scaling.c_e.cwiseProduct(y) * inv_d_f;
+
+  return kkt_error<Scalar, T>(g_unscaled, A_e_unscaled, c_e_unscaled,
+                              y_unscaled);
+}
+
+/// Returns the unscaled KKT error for the interior-point method.
+///
+/// @tparam Scalar Scalar type.
+/// @tparam T The type of KKT error to compute.
+/// @param scaling The problem scaling factors.
+/// @param g Gradient of the scaled cost function d_f·∇f.
+/// @param A_e The problem's scaled equality constraint Jacobian D_cₑ·Aₑ(x)
+///     evaluated at the current iterate.
+/// @param c_e The problem's scaled equality constraints D_cₑ·cₑ(x) evaluated at
+///     the current iterate.
+/// @param A_i The problem's scaled inequality constraint Jacobian D_cᵢ·Aᵢ(x)
+///     evaluated at the current iterate.
+/// @param c_i The problem's scaled inequality constraints D_cᵢ·cᵢ(x) evaluated
+///     at the current iterate.
+/// @param s Scaled inequality constraint slack variables.
+/// @param y Scaled equality constraint dual variables.
+/// @param z Scaled inequality constraint dual variables.
+/// @param μ Scaled barrier parameter.
+template <typename Scalar, KKTErrorType T>
+Scalar unscaled_kkt_error(const ProblemScaling<Scalar>& scaling,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
+                          const Eigen::SparseMatrix<Scalar>& A_e,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e,
+                          const Eigen::SparseMatrix<Scalar>& A_i,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& c_i,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& s,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& y,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& z,
+                          Scalar μ) {
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
+
+  if (scaling.is_identity()) {
+    return kkt_error<Scalar, T>(g, A_e, c_e, A_i, c_i, s, y, z, μ);
+  }
+
+  const Scalar inv_d_f = Scalar(1) / scaling.f;
+  const DenseVector inv_d_c_e = scaling.c_e.cwiseInverse();
+  const DenseVector inv_d_c_i = scaling.c_i.cwiseInverse();
+
+  const DenseVector g_unscaled = inv_d_f * g;
+  const SparseMatrix A_e_unscaled = inv_d_c_e.asDiagonal() * A_e;
+  const DenseVector c_e_unscaled = inv_d_c_e.cwiseProduct(c_e);
+  const SparseMatrix A_i_unscaled = inv_d_c_i.asDiagonal() * A_i;
+  const DenseVector c_i_unscaled = inv_d_c_i.cwiseProduct(c_i);
+  const DenseVector s_unscaled = inv_d_c_i.cwiseProduct(s);
+  const DenseVector y_unscaled = scaling.c_e.cwiseProduct(y) * inv_d_f;
+  const DenseVector z_unscaled = scaling.c_i.cwiseProduct(z) * inv_d_f;
+  const Scalar μ_unscaled = inv_d_f * μ;
+
+  return kkt_error<Scalar, T>(g_unscaled, A_e_unscaled, c_e_unscaled,
+                              A_i_unscaled, c_i_unscaled, s_unscaled,
+                              y_unscaled, z_unscaled, μ_unscaled);
+}
+
 }  // namespace slp

sleipnirgroup_jormungandr-0.5.6.dev7/include/sleipnir/optimization/solver/util/problem_scaling.hpp

@@ -0,0 +1,116 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <algorithm>
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
+
+#include "sleipnir/optimization/solver/util/sparse_inf_norms.hpp"
+
+// See docs/algorithms.md#Works_cited for citation definitions
+
+namespace slp {
+
+/// Automatic problem scaling factors.
+///
+/// @tparam Scalar Scalar type.
+template <typename Scalar>
+struct ProblemScaling {
+  /// Type alias for dense vector.
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+  /// Type alias for sparse matrix.
+  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
+  /// Type alias for sparse vector.
+  using SparseVector = Eigen::SparseVector<Scalar>;
+
+  /// Cost scaling factor d_f.
+  Scalar f = Scalar(1);
+
+  /// Equality constraint scaling factors d_cₑ.
+  DenseVector c_e;
+
+  /// Inequality constraint scaling factors d_cᵢ.
+  DenseVector c_i;
+
+  /// Constructs identity problem scaling.
+  ProblemScaling() = default;
+
+  /// Constructs problem scaling from explicit factors.
+  ///
+  /// @param f Cost scaling factor d_f.
+  /// @param c_e Equality constraint scaling factors d_cₑ.
+  /// @param c_i Inequality constraint scaling factors d_cᵢ.
+  ProblemScaling(Scalar f, const DenseVector& c_e, const DenseVector& c_i)
+      : f{f}, c_e{c_e}, c_i{c_i} {}
+
+  /// Computes Newton problem scaling.
+  ///
+  /// Scales the cost so the largest gradient component at the starting point
+  /// is at most gₘₐₓ:
+  ///
+  ///   d_f = min(1, gₘₐₓ / ‖∇f(x₀)‖_∞)
+  ///
+  /// See §3.8 Automatic Scaling of the Problem Statement in [2].
+  ///
+  /// @param g Cost gradient ∇f, evaluated at the starting point.
+  explicit ProblemScaling(const DenseVector& g) {
+    constexpr Scalar g_max(100);
+
+    f = std::min(Scalar(1), g_max / g.template lpNorm<Eigen::Infinity>());
+  }
+
+  /// Computes SQP problem scaling.
+  ///
+  /// Scales the cost and each equality constraint so the largest gradient
+  /// component at the starting point is at most gₘₐₓ:
+  ///
+  ///   d_f     = min(1, gₘₐₓ / ‖∇f(x₀)‖_∞)
+  ///   d_cₑ[j] = min(1, gₘₐₓ / ‖∇cₑⱼ(x₀)‖_∞)
+  ///
+  /// See §3.8 Automatic Scaling of the Problem Statement in [2].
+  ///
+  /// @param g Cost gradient ∇f, evaluated at the starting point.
+  /// @param A_e Equality constraint Jacobian Aₑ, evaluated at the starting
+  ///     point.
+  ProblemScaling(const DenseVector& g, const SparseMatrix& A_e) {
+    constexpr Scalar g_max(100);
+
+    f = std::min(Scalar(1), g_max / g.template lpNorm<Eigen::Infinity>());
+    c_e = (g_max / sparse_inf_norms(A_e).array()).min(Scalar(1)).matrix();
+  }
+
+  /// Computes interior-point problem scaling.
+  ///
+  /// Scales the cost and each constraint so the largest gradient
+  /// component at the starting point is at most gₘₐₓ:
+  ///
+  ///   d_f    = min(1, gₘₐₓ / ‖∇f(x₀)‖_∞)
+  ///   d_c[j] = min(1, gₘₐₓ / ‖∇cⱼ(x₀)‖_∞)
+  ///
+  /// See §3.8 Automatic Scaling of the Problem Statement in [2].
+  ///
+  /// @param g Cost gradient ∇f, evaluated at the starting point.
+  /// @param A_e Equality constraint Jacobian Aₑ, evaluated at the starting
+  ///     point.
+  /// @param A_i Inequality constraint Jacobian Aᵢ, evaluated at the starting
+  ///     point.
+  ProblemScaling(const DenseVector& g, const SparseMatrix& A_e,
+                 const SparseMatrix& A_i) {
+    constexpr Scalar g_max(100);
+
+    f = std::min(Scalar(1), g_max / g.template lpNorm<Eigen::Infinity>());
+    c_e = (g_max / sparse_inf_norms(A_e).array()).min(Scalar(1)).matrix();
+    c_i = (g_max / sparse_inf_norms(A_i).array()).min(Scalar(1)).matrix();
+  }
+
+  /// Whether the problem scaling is identity.
+  ///
+  /// @return True if the problem scaling is identity.
+  bool is_identity() const {
+    return f == Scalar(1) && c_e.size() == 0 && c_i.size() == 0;
+  }
+};
+
+}  // namespace slp

sleipnirgroup_jormungandr-0.5.6.dev7/include/sleipnir/optimization/solver/util/sparse_inf_norms.hpp

@@ -0,0 +1,34 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <algorithm>
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
+
+namespace slp {
+
+/// Computes the row-wise infinity norm of a sparse matrix.
+///
+/// @param mat Sparse matrix.
+/// @return A dense vector where each element is the infinity norm of the
+///         corresponding row of the sparse matrix.
+template <typename Derived>
+auto sparse_inf_norms(const Eigen::SparseCompressedBase<Derived>& mat)
+    -> Eigen::Vector<typename Derived::Scalar, Eigen::Dynamic> {
+  using Scalar = typename Derived::Scalar;
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+
+  DenseVector norms = DenseVector::Zero(mat.rows());
+  for (int k = 0; k < mat.outerSize(); ++k) {
+    for (typename Derived::InnerIterator it{mat, k}; it; ++it) {
+      using std::abs;
+      norms[it.row()] = std::max(norms[it.row()], abs(it.value()));
+    }
+  }
+
+  return norms;
+}
+
+}  // namespace slp

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

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