Use Newton's method on unconstrained problems (#658)

include/sleipnir/optimization/OptimizationProblem.hpp

@@ -19,6 +19,7 @@
 #include "sleipnir/optimization/SolverIterationInfo.hpp"
 #include "sleipnir/optimization/SolverStatus.hpp"
 #include "sleipnir/optimization/solver/InteriorPoint.hpp"
+#include "sleipnir/optimization/solver/Newton.hpp"
 #include "sleipnir/optimization/solver/SQP.hpp"
 #include "sleipnir/util/Print.hpp"
 #include "sleipnir/util/SymbolExports.hpp"
@@ -306,7 +307,9 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
     }
 
     // Solve the optimization problem
-    if (m_inequalityConstraints.empty()) {
+    if (m_equalityConstraints.empty() && m_inequalityConstraints.empty()) {
+      Newton(m_decisionVariables, m_f.value(), m_callback, config, x, &status);
+    } else if (m_inequalityConstraints.empty()) {
       SQP(m_decisionVariables, m_equalityConstraints, m_f.value(), m_callback,
           config, x, &status);
     } else {

include/sleipnir/optimization/solver/Newton.hpp

@@ -0,0 +1,42 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <span>
+
+#include <Eigen/Core>
+
+#include "sleipnir/autodiff/Variable.hpp"
+#include "sleipnir/optimization/SolverConfig.hpp"
+#include "sleipnir/optimization/SolverIterationInfo.hpp"
+#include "sleipnir/optimization/SolverStatus.hpp"
+#include "sleipnir/util/FunctionRef.hpp"
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+/**
+Finds the optimal solution to a nonlinear program using Newton's method.
+
+A nonlinear program has the form:
+
+@verbatim
+     min_x f(x)
+@endverbatim
+
+where f(x) is the cost function.
+
+@param[in] decisionVariables The list of decision variables.
+@param[in] f The cost function.
+@param[in] callback The user callback.
+@param[in] config Configuration options for the solver.
+@param[in,out] x The initial guess and output location for the decision
+  variables.
+@param[out] status The solver status.
+*/
+SLEIPNIR_DLLEXPORT void Newton(
+    std::span<Variable> decisionVariables, Variable& f,
+    function_ref<bool(const SolverIterationInfo& info)> callback,
+    const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status);
+
+}  // namespace sleipnir

jormungandr/cpp/Docstrings.hpp

@@ -364,6 +364,34 @@ static const char *__doc_sleipnir_MultistartResult_status = R"doc()doc";
 
 static const char *__doc_sleipnir_MultistartResult_variables = R"doc()doc";
 
+static const char *__doc_sleipnir_Newton =
+R"doc(Finds the optimal solution to a nonlinear program using Newton's
+method.
+
+A nonlinear program has the form:
+
+@verbatim min_x f(x) @endverbatim
+
+where f(x) is the cost function.
+
+Parameter ``decision_variables``:
+    The list of decision variables.
+
+Parameter ``f``:
+    The cost function.
+
+Parameter ``callback``:
+    The user callback.
+
+Parameter ``config``:
+    Configuration options for the solver.
+
+Parameter ``x``:
+    The initial guess and output location for the decision variables.
+
+Parameter ``status``:
+    The solver status.)doc";
+
 static const char *__doc_sleipnir_OCPSolver =
 R"doc(This class allows the user to pose and solve a constrained optimal
 control problem (OCP) in a variety of ways.

src/optimization/solver/Newton.cpp

@@ -0,0 +1,289 @@
+// Copyright (c) Sleipnir contributors
+
+#include "sleipnir/optimization/solver/Newton.hpp"
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <fstream>
+#include <limits>
+
+#include <Eigen/SparseCholesky>
+
+#include "optimization/RegularizedLDLT.hpp"
+#include "optimization/solver/util/ErrorEstimate.hpp"
+#include "optimization/solver/util/Filter.hpp"
+#include "optimization/solver/util/KKTError.hpp"
+#include "sleipnir/autodiff/Gradient.hpp"
+#include "sleipnir/autodiff/Hessian.hpp"
+#include "sleipnir/optimization/SolverExitCondition.hpp"
+#include "sleipnir/util/Print.hpp"
+#include "sleipnir/util/Spy.hpp"
+#include "util/ScopeExit.hpp"
+#include "util/ToMilliseconds.hpp"
+
+// See docs/algorithms.md#Works_cited for citation definitions.
+
+namespace sleipnir {
+
+void Newton(std::span<Variable> decisionVariables, Variable& f,
+            function_ref<bool(const SolverIterationInfo& info)> callback,
+            const SolverConfig& config, Eigen::VectorXd& x,
+            SolverStatus* status) {
+  const auto solveStartTime = std::chrono::system_clock::now();
+
+  // Map decision variables and constraints to VariableMatrices for Lagrangian
+  VariableMatrix xAD{decisionVariables};
+  xAD.SetValue(x);
+
+  // Lagrangian L
+  //
+  // L(xₖ, yₖ) = f(xₖ)
+  auto L = f;
+
+  // Gradient of f ∇f
+  Gradient gradientF{f, xAD};
+  Eigen::SparseVector<double> g = gradientF.Value();
+
+  // Hessian of the Lagrangian H
+  //
+  // Hₖ = ∇²ₓₓL(xₖ, yₖ)
+  Hessian hessianL{L, xAD};
+  Eigen::SparseMatrix<double> H = hessianL.Value();
+
+  // Check whether initial guess has finite f(xₖ)
+  if (!std::isfinite(f.Value())) {
+    status->exitCondition =
+        SolverExitCondition::kNonfiniteInitialCostOrConstraints;
+    return;
+  }
+
+  // Sparsity pattern files written when spy flag is set in SolverConfig
+  std::ofstream H_spy;
+  if (config.spy) {
+    H_spy.open("H.spy");
+  }
+
+  if (config.diagnostics) {
+    sleipnir::println("Error tolerance: {}\n", config.tolerance);
+  }
+
+  std::chrono::system_clock::time_point iterationsStartTime;
+
+  int iterations = 0;
+
+  // Prints final diagnostics when the solver exits
+  scope_exit exit{[&] {
+    status->cost = f.Value();
+
+    if (config.diagnostics) {
+      auto solveEndTime = std::chrono::system_clock::now();
+
+      sleipnir::println("\nSolve time: {:.3f} ms",
+                        ToMilliseconds(solveEndTime - solveStartTime));
+      sleipnir::println("  ↳ {:.3f} ms (solver setup)",
+                        ToMilliseconds(iterationsStartTime - solveStartTime));
+      if (iterations > 0) {
+        sleipnir::println(
+            "  ↳ {:.3f} ms ({} solver iterations; {:.3f} ms average)",
+            ToMilliseconds(solveEndTime - iterationsStartTime), iterations,
+            ToMilliseconds((solveEndTime - iterationsStartTime) / iterations));
+      }
+      sleipnir::println("");
+
+      sleipnir::println("{:^8}   {:^10}   {:^14}   {:^6}", "autodiff",
+                        "setup (ms)", "avg solve (ms)", "solves");
+      sleipnir::println("{:=^47}", "");
+      constexpr auto format = "{:^8}   {:10.3f}   {:14.3f}   {:6}";
+      sleipnir::println(format, "∇f(x)",
+                        gradientF.GetProfiler().SetupDuration(),
+                        gradientF.GetProfiler().AverageSolveDuration(),
+                        gradientF.GetProfiler().SolveMeasurements());
+      sleipnir::println(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(),
+                        hessianL.GetProfiler().AverageSolveDuration(),
+                        hessianL.GetProfiler().SolveMeasurements());
+      sleipnir::println("");
+    }
+  }};
+
+  Filter filter{f};
+
+  RegularizedLDLT solver;
+
+  // Variables for determining when a step is acceptable
+  constexpr double α_red_factor = 0.5;
+  int acceptableIterCounter = 0;
+
+  // Error estimate
+  double E_0 = std::numeric_limits<double>::infinity();
+
+  if (config.diagnostics) {
+    iterationsStartTime = std::chrono::system_clock::now();
+  }
+
+  while (E_0 > config.tolerance &&
+         acceptableIterCounter < config.maxAcceptableIterations) {
+    std::chrono::system_clock::time_point innerIterStartTime;
+    if (config.diagnostics) {
+      innerIterStartTime = std::chrono::system_clock::now();
+    }
+
+    // Check for diverging iterates
+    if (x.lpNorm<Eigen::Infinity>() > 1e20 || !x.allFinite()) {
+      status->exitCondition = SolverExitCondition::kDivergingIterates;
+      return;
+    }
+
+    // Write out spy file contents if that's enabled
+    if (config.spy) {
+      // Gap between sparsity patterns
+      if (iterations > 0) {
+        H_spy << "\n";
+      }
+
+      Spy(H_spy, H);
+    }
+
+    // Call user callback
+    if (callback({iterations, x, Eigen::VectorXd::Zero(0), g, H,
+                  Eigen::SparseMatrix<double>{},
+                  Eigen::SparseMatrix<double>{}})) {
+      status->exitCondition = SolverExitCondition::kCallbackRequestedStop;
+      return;
+    }
+
+    // rhs = −[∇f]
+    Eigen::VectorXd rhs = -g;
+
+    // Solve the Newton-KKT system
+    //
+    // [H][ pₖˣ] = −[∇f]
+    solver.Compute(H, 0, config.tolerance / 10.0);
+    Eigen::VectorXd step = solver.Solve(rhs);
+
+    // step = [ pₖˣ]
+    Eigen::VectorXd p_x = step.segment(0, x.rows());
+
+    constexpr double α_max = 1.0;
+    double α = α_max;
+
+    // Loop until a step is accepted. If a step becomes acceptable, the loop
+    // will exit early.
+    while (1) {
+      Eigen::VectorXd trial_x = x + α * p_x;
+
+      xAD.SetValue(trial_x);
+
+      // If f(xₖ + αpₖˣ) isn't finite, reduce step size immediately
+      if (!std::isfinite(f.Value())) {
+        // Reduce step size
+        α *= α_red_factor;
+        continue;
+      }
+
+      // Check whether filter accepts trial iterate
+      auto entry = filter.MakeEntry();
+      if (filter.TryAdd(entry)) {
+        // Accept step
+        break;
+      }
+
+      // Reduce step size
+      α *= α_red_factor;
+
+      // Safety factor for the minimal step size
+      constexpr double α_min_frac = 0.05;
+
+      // If step size hit a minimum, check if the KKT error was reduced. If it
+      // wasn't, report infeasible.
+      if (α < α_min_frac * Filter::γConstraint) {
+        double currentKKTError = KKTError(g);
+
+        Eigen::VectorXd trial_x = x + α_max * p_x;
+
+        // Upate autodiff
+        xAD.SetValue(trial_x);
+
+        double nextKKTError = KKTError(gradientF.Value());
+
+        // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
+        if (nextKKTError <= 0.999 * currentKKTError) {
+          α = α_max;
+
+          // Accept step
+          break;
+        }
+
+        status->exitCondition = SolverExitCondition::kLocallyInfeasible;
+        return;
+      }
+    }
+
+    // Handle very small search directions by letting αₖ = αₖᵐᵃˣ when
+    // max(|pₖˣ(i)|/(1 + |xₖ(i)|)) < 10ε_mach.
+    //
+    // See section 3.9 of [2].
+    double maxStepScaled = 0.0;
+    for (int row = 0; row < x.rows(); ++row) {
+      maxStepScaled = std::max(maxStepScaled,
+                               std::abs(p_x(row)) / (1.0 + std::abs(x(row))));
+    }
+    if (maxStepScaled < 10.0 * std::numeric_limits<double>::epsilon()) {
+      α = α_max;
+    }
+
+    // xₖ₊₁ = xₖ + αₖpₖˣ
+    x += α * p_x;
+
+    // Update autodiff for Hessian
+    xAD.SetValue(x);
+    g = gradientF.Value();
+    H = hessianL.Value();
+
+    // Update the error estimate
+    E_0 = ErrorEstimate(g);
+    if (E_0 < config.acceptableTolerance) {
+      ++acceptableIterCounter;
+    } else {
+      acceptableIterCounter = 0;
+    }
+
+    const auto innerIterEndTime = std::chrono::system_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);
+    }
+
+    ++iterations;
+
+    // Check for max iterations
+    if (iterations >= config.maxIterations) {
+      status->exitCondition = SolverExitCondition::kMaxIterationsExceeded;
+      return;
+    }
+
+    // Check for max wall clock time
+    if (innerIterEndTime - solveStartTime > config.timeout) {
+      status->exitCondition = SolverExitCondition::kTimeout;
+      return;
+    }
+
+    // Check for solve to acceptable tolerance
+    if (E_0 > config.tolerance &&
+        acceptableIterCounter == config.maxAcceptableIterations) {
+      status->exitCondition = SolverExitCondition::kSolvedToAcceptableTolerance;
+      return;
+    }
+  }
+}
+
+}  // namespace sleipnir

src/optimization/solver/SQP.cpp

@@ -383,7 +383,7 @@ void SQP(std::span<Variable> decisionVariables,
       constexpr double α_min_frac = 0.05;
 
       // If step size hit a minimum, check if the KKT error was reduced. If it
-      // wasn't, report infeasible.
+      // wasn't, invoke feasibility restoration.
       if (α < α_min_frac * Filter::γConstraint) {
         double currentKKTError = KKTError(g, A_e, c_e, y);