Add regularization and step size to diagnostics

SleipnirGroup · Jan 7, 2025 · 8057ee8 · 8057ee8
1 parent 5af8519
commit 8057ee8
Show file tree

Hide file tree

Showing 5 changed files with 71 additions and 33 deletions.
diff --git a/src/optimization/RegularizedLDLT.hpp b/src/optimization/RegularizedLDLT.hpp
@@ -115,7 +115,7 @@ class RegularizedLDLT {
   }
 
   /**
-   * Solve the system of equations using a regularized LDLT factorization.
+   * Solves the system of equations using a regularized LDLT factorization.
    *
    * @param rhs Right-hand side of the system.
    */
@@ -124,6 +124,11 @@ class RegularizedLDLT {
     return m_solver.solve(rhs);
   }
 
+  /**
+   * Returns the Hessian regularization factor.
+   */
+  double HessianRegularization() const { return m_δOld; }
+
  private:
   Solver m_solver;
 

diff --git a/src/optimization/solver/InteriorPoint.cpp b/src/optimization/solver/InteriorPoint.cpp
@@ -24,6 +24,7 @@
 #include "sleipnir/util/Print.hpp"
 #include "sleipnir/util/Spy.hpp"
 #include "sleipnir/util/small_vector.hpp"
+#include "util/PrintIterationDiagnostics.hpp"
 #include "util/ScopeExit.hpp"
 #include "util/ToMilliseconds.hpp"
 
@@ -786,19 +787,12 @@ void InteriorPoint(std::span<Variable> decisionVariables,
 
     const auto innerIterEndTime = std::chrono::steady_clock::now();
 
-    // Diagnostics for current iteration
     if (config.diagnostics) {
-      if (iterations % 20 == 0) {
-        sleipnir::println("{:^4}   {:^9}  {:^13}  {:^13}  {:^13}", "iter",
-                          "time (ms)", "error", "cost", "infeasibility");
-        sleipnir::println("{:=^61}", "");
-      }
-
-      sleipnir::println("{:4}{}  {:9.3f}  {:13e}  {:13e}  {:13e}", iterations,
-                        feasibilityRestoration ? "r" : " ",
-                        ToMilliseconds(innerIterEndTime - innerIterStartTime),
-                        E_0, f.Value(),
-                        c_e.lpNorm<1>() + (c_i - s).lpNorm<1>());
+      PrintIterationDiagnostics(iterations, feasibilityRestoration,
+                                innerIterEndTime - innerIterStartTime, E_0,
+                                f.Value(),
+                                c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(),
+                                solver.HessianRegularization(), α);
     }
 
     ++iterations;

diff --git a/src/optimization/solver/Newton.cpp b/src/optimization/solver/Newton.cpp
@@ -19,6 +19,7 @@
 #include "sleipnir/optimization/SolverExitCondition.hpp"
 #include "sleipnir/util/Print.hpp"
 #include "sleipnir/util/Spy.hpp"
+#include "util/PrintIterationDiagnostics.hpp"
 #include "util/ScopeExit.hpp"
 #include "util/ToMilliseconds.hpp"
 
@@ -250,17 +251,10 @@ void Newton(std::span<Variable> decisionVariables, Variable& f,
 
     const auto innerIterEndTime = std::chrono::steady_clock::now();
 
-    // Diagnostics for current iteration
     if (config.diagnostics) {
-      if (iterations % 20 == 0) {
-        sleipnir::println("{:^4}   {:^9}  {:^13}  {:^13}  {:^13}", "iter",
-                          "time (ms)", "error", "cost", "infeasibility");
-        sleipnir::println("{:=^61}", "");
-      }
-
-      sleipnir::println("{:4}   {:9.3f}  {:13e}  {:13e}  {:13e}", iterations,
-                        ToMilliseconds(innerIterEndTime - innerIterStartTime),
-                        E_0, f.Value(), 0.0);
+      PrintIterationDiagnostics(
+          iterations, false, innerIterEndTime - innerIterStartTime, E_0,
+          f.Value(), 0.0, solver.HessianRegularization(), α);
     }
 
     ++iterations;

diff --git a/src/optimization/solver/SQP.cpp b/src/optimization/solver/SQP.cpp
@@ -23,6 +23,7 @@
 #include "sleipnir/util/Print.hpp"
 #include "sleipnir/util/Spy.hpp"
 #include "sleipnir/util/small_vector.hpp"
+#include "util/PrintIterationDiagnostics.hpp"
 #include "util/ScopeExit.hpp"
 #include "util/ToMilliseconds.hpp"
 
@@ -520,17 +521,10 @@ void SQP(std::span<Variable> decisionVariables,
 
     const auto innerIterEndTime = std::chrono::steady_clock::now();
 
-    // Diagnostics for current iteration
     if (config.diagnostics) {
-      if (iterations % 20 == 0) {
-        sleipnir::println("{:^4}   {:^9}  {:^13}  {:^13}  {:^13}", "iter",
-                          "time (ms)", "error", "cost", "infeasibility");
-        sleipnir::println("{:=^61}", "");
-      }
-
-      sleipnir::println("{:4}   {:9.3f}  {:13e}  {:13e}  {:13e}", iterations,
-                        ToMilliseconds(innerIterEndTime - innerIterStartTime),
-                        E_0, f.Value(), c_e.lpNorm<1>());
+      PrintIterationDiagnostics(
+          iterations, false, innerIterEndTime - innerIterStartTime, E_0,
+          f.Value(), c_e.lpNorm<1>(), solver.HessianRegularization(), α);
     }
 
     ++iterations;

diff --git a/src/util/PrintIterationDiagnostics.hpp b/src/util/PrintIterationDiagnostics.hpp
@@ -0,0 +1,51 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <chrono>
+#include <cmath>
+
+#include "sleipnir/util/Print.hpp"
+#include "util/ToMilliseconds.hpp"
+
+namespace sleipnir {
+
+/**
+ * Prints diagnostics for the current iteration.
+ *
+ * @param iterations Number of iterations.
+ * @param feasibilityRestoration Whether solver is in feasibility restoration
+ *     mode.
+ * @param time The iteration duration.
+ * @param error The error.
+ * @param cost The cost.
+ * @param infeasibility The infeasibility.
+ * @param δ The Hessian regularization factor.
+ * @param α The step size.
+ */
+template <typename Rep, typename Period = std::ratio<1>>
+void PrintIterationDiagnostics(int iterations, bool feasibilityRestoration,
+                               const std::chrono::duration<Rep, Period>& time,
+                               double error, double cost, double infeasibility,
+                               double δ, double α) {
+  if (iterations % 20 == 0) {
+    sleipnir::println("{:^4}   {:^9}  {:^13}  {:^13}  {:^13}  {:^8}  {:^8}",
+                      "iter", "time (ms)", "error", "cost", "infeasibility",
+                      "log10(δ)", "-log2(α)");  // NOLINT
+    sleipnir::println("{:=^81}", "");
+  }
+
+  sleipnir::print("{:4}{}  {:9.3f}  {:13e}  {:13e}  {:13e}  ", iterations,
+                  feasibilityRestoration ? "r" : " ", ToMilliseconds(time),
+                  error, cost, infeasibility);
+
+  if (δ > 0.0) {
+    sleipnir::print("{:8.2f}  ", std::log10(δ));
+  } else {
+    sleipnir::print("{:>8}  ", "-∞");
+  }
+
+  sleipnir::println("{:8d}", static_cast<int>(-std::log2(α)));
+}
+
+}  // namespace sleipnir