Use SQP for problems without inequality constraints (#655)

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/SQP.hpp"
 #include "sleipnir/util/Print.hpp"
 #include "sleipnir/util/SymbolExports.hpp"
 #include "sleipnir/util/small_vector.hpp"
@@ -305,10 +306,15 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
     }
 
     // Solve the optimization problem
-    Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size());
-    InteriorPoint(m_decisionVariables, m_equalityConstraints,
-                  m_inequalityConstraints, m_f.value(), m_callback, config,
-                  false, x, s, &status);
+    if (m_inequalityConstraints.empty()) {
+      SQP(m_decisionVariables, m_equalityConstraints, m_f.value(), m_callback,
+          config, x, &status);
+    } else {
+      Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size());
+      InteriorPoint(m_decisionVariables, m_equalityConstraints,
+                    m_inequalityConstraints, m_f.value(), m_callback, config,
+                    false, x, s, &status);
+    }
 
     if (config.diagnostics) {
       sleipnir::println("Exit condition: {}", ToMessage(status.exitCondition));

include/sleipnir/optimization/solver/SQP.hpp

@@ -0,0 +1,46 @@
+// 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 Sequential Quadratic
+Programming (SQP).
+
+A nonlinear program has the form:
+
+@verbatim
+     min_x f(x)
+subject to cₑ(x) = 0
+@endverbatim
+
+where f(x) is the cost function and cₑ(x) are the equality constraints.
+
+@param[in] decisionVariables The list of decision variables.
+@param[in] equalityConstraints The list of equality constraints.
+@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 SQP(
+    std::span<Variable> decisionVariables,
+    std::span<Variable> equalityConstraints, Variable& f,
+    function_ref<bool(const SolverIterationInfo& info)> callback,
+    const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status);
+
+}  // namespace sleipnir

jormungandr/cpp/Docstrings.hpp

@@ -774,6 +774,38 @@ Parameter ``t0``:
 Parameter ``dt``:
     The time over which to integrate.)doc";
 
+static const char *__doc_sleipnir_SQP =
+R"doc(Finds the optimal solution to a nonlinear program using Sequential
+Quadratic Programming (SQP).
+
+A nonlinear program has the form:
+
+@verbatim min_x f(x) subject to cₑ(x) = 0 @endverbatim
+
+where f(x) is the cost function and cₑ(x) are the equality
+constraints.
+
+Parameter ``decision_variables``:
+    The list of decision variables.
+
+Parameter ``equality_constraints``:
+    The list of equality constraints.
+
+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_Slice = R"doc()doc";
 
 static const char *__doc_sleipnir_Slice_Adjust =