sleipnirgroup-jormungandr 0.0.1.dev475__tar.gz → 0.0.1.dev477__tar.gz

{sleipnirgroup_jormungandr-0.0.1.dev475 → sleipnirgroup_jormungandr-0.0.1.dev477}/CMakeLists.txt

@@ -71,6 +71,11 @@ option(BUILD_BENCHMARKS "Build CasADi and Sleipnir benchmarks" OFF)
 option(BUILD_EXAMPLES "Build examples" OFF)
 option(BUILD_PYTHON "Build Python module" OFF)
 option(DISABLE_DIAGNOSTICS "Disable diagnostics support at compile-time" OFF)
+option(
+    ENABLE_BOUND_PROJECTION
+    "Enable projecting the state onto bound inequality constraints inside problem setup"
+    OFF
+)
 
 include(CompilerFlags)
 
@@ -104,6 +109,10 @@ if(DISABLE_DIAGNOSTICS)
     target_compile_definitions(Sleipnir PUBLIC SLEIPNIR_DISABLE_DIAGNOSTICS)
 endif()
 
+if(ENABLE_BOUND_PROJECTION)
+    target_compile_definitions(Sleipnir PUBLIC SLEIPNIR_ENABLE_BOUND_PROJECTION)
+endif()
+
 # Eigen dependency
 if(NOT USE_SYSTEM_EIGEN)
     set(EIGEN_BUILD_CMAKE_PACKAGE TRUE)

{sleipnirgroup_jormungandr-0.0.1.dev475 → sleipnirgroup_jormungandr-0.0.1.dev477}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: sleipnirgroup-jormungandr
-Version: 0.0.1.dev475
+Version: 0.0.1.dev477
 Summary: A linearity-exploiting sparse nonlinear constrained optimization problem solver that uses the interior-point method.
 License: Copyright (c) Sleipnir contributors
         

{sleipnirgroup_jormungandr-0.0.1.dev475 → sleipnirgroup_jormungandr-0.0.1.dev477}/include/sleipnir/autodiff/expression.hpp

@@ -436,9 +436,7 @@ struct BinaryMinusExpression final : Expression {
    * @param rhs Binary operator's right operand.
    */
   constexpr BinaryMinusExpression(ExpressionPtr lhs, ExpressionPtr rhs)
-      : Expression{std::move(lhs), std::move(rhs)} {
-    val = args[0]->val - args[1]->val;
-  }
+      : Expression{std::move(lhs), std::move(rhs)} {}
 
   double value(double lhs, double rhs) const override { return lhs - rhs; }
 
@@ -479,9 +477,7 @@ struct BinaryPlusExpression final : Expression {
    * @param rhs Binary operator's right operand.
    */
   constexpr BinaryPlusExpression(ExpressionPtr lhs, ExpressionPtr rhs)
-      : Expression{std::move(lhs), std::move(rhs)} {
-    val = args[0]->val + args[1]->val;
-  }
+      : Expression{std::move(lhs), std::move(rhs)} {}
 
   double value(double lhs, double rhs) const override { return lhs + rhs; }
 
@@ -565,9 +561,7 @@ struct DivExpression final : Expression {
    * @param rhs Binary operator's right operand.
    */
   constexpr DivExpression(ExpressionPtr lhs, ExpressionPtr rhs)
-      : Expression{std::move(lhs), std::move(rhs)} {
-    val = args[0]->val / args[1]->val;
-  }
+      : Expression{std::move(lhs), std::move(rhs)} {}
 
   double value(double lhs, double rhs) const override { return lhs / rhs; }
 
@@ -608,9 +602,7 @@ struct MultExpression final : Expression {
    * @param rhs Binary operator's right operand.
    */
   constexpr MultExpression(ExpressionPtr lhs, ExpressionPtr rhs)
-      : Expression{std::move(lhs), std::move(rhs)} {
-    val = args[0]->val * args[1]->val;
-  }
+      : Expression{std::move(lhs), std::move(rhs)} {}
 
   double value(double lhs, double rhs) const override { return lhs * rhs; }
 
@@ -652,9 +644,7 @@ struct UnaryMinusExpression final : Expression {
    * @param lhs Unary operator's operand.
    */
   explicit constexpr UnaryMinusExpression(ExpressionPtr lhs)
-      : Expression{std::move(lhs)} {
-    val = -args[0]->val;
-  }
+      : Expression{std::move(lhs)} {}
 
   double value(double lhs, double) const override { return -lhs; }
 
@@ -735,9 +725,7 @@ struct AbsExpression final : Expression {
    * @param lhs Unary operator's operand.
    */
   explicit constexpr AbsExpression(ExpressionPtr lhs)
-      : Expression{std::move(lhs)} {
-    val = args[0]->val < 0 ? -args[0]->val : args[0]->val;
-  }
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override { return std::abs(x); }
 
@@ -798,9 +786,8 @@ struct AcosExpression final : Expression {
    *
    * @param lhs Unary operator's operand.
    */
-  explicit AcosExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
-    val = std::acos(args[0]->val);
-  }
+  explicit constexpr AcosExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override { return std::acos(x); }
 
@@ -848,9 +835,8 @@ struct AsinExpression final : Expression {
    *
    * @param lhs Unary operator's operand.
    */
-  explicit AsinExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
-    val = std::asin(args[0]->val);
-  }
+  explicit constexpr AsinExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override { return std::asin(x); }
 
@@ -899,9 +885,8 @@ struct AtanExpression final : Expression {
    *
    * @param lhs Unary operator's operand.
    */
-  explicit AtanExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
-    val = std::atan(args[0]->val);
-  }
+  explicit constexpr AtanExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override { return std::atan(x); }
 
@@ -950,10 +935,8 @@ struct Atan2Expression final : Expression {
    * @param lhs Binary operator's left operand.
    * @param rhs Binary operator's right operand.
    */
-  Atan2Expression(ExpressionPtr lhs, ExpressionPtr rhs)
-      : Expression{std::move(lhs), std::move(rhs)} {
-    val = std::atan2(args[0]->val, args[1]->val);
-  }
+  constexpr Atan2Expression(ExpressionPtr lhs, ExpressionPtr rhs)
+      : Expression{std::move(lhs), std::move(rhs)} {}
 
   double value(double y, double x) const override { return std::atan2(y, x); }
 
@@ -1014,9 +997,8 @@ struct CosExpression final : Expression {
    *
    * @param lhs Unary operator's operand.
    */
-  explicit CosExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
-    val = std::cos(args[0]->val);
-  }
+  explicit constexpr CosExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override { return std::cos(x); }
 
@@ -1063,9 +1045,8 @@ struct CoshExpression final : Expression {
    *
    * @param lhs Unary operator's operand.
    */
-  explicit CoshExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
-    val = std::cosh(args[0]->val);
-  }
+  explicit constexpr CoshExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override { return std::cosh(x); }
 
@@ -1112,9 +1093,8 @@ struct ErfExpression final : Expression {
    *
    * @param lhs Unary operator's operand.
    */
-  explicit ErfExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
-    val = std::erf(args[0]->val);
-  }
+  explicit constexpr ErfExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override { return std::erf(x); }
 
@@ -1165,9 +1145,8 @@ struct ExpExpression final : Expression {
    *
    * @param lhs Unary operator's operand.
    */
-  explicit ExpExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
-    val = std::exp(args[0]->val);
-  }
+  explicit constexpr ExpExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override { return std::exp(x); }
 
@@ -1217,10 +1196,8 @@ struct HypotExpression final : Expression {
    * @param lhs Binary operator's left operand.
    * @param rhs Binary operator's right operand.
    */
-  HypotExpression(ExpressionPtr lhs, ExpressionPtr rhs)
-      : Expression{std::move(lhs), std::move(rhs)} {
-    val = std::hypot(args[0]->val, args[1]->val);
-  }
+  constexpr HypotExpression(ExpressionPtr lhs, ExpressionPtr rhs)
+      : Expression{std::move(lhs), std::move(rhs)} {}
 
   double value(double x, double y) const override { return std::hypot(x, y); }
 
@@ -1280,9 +1257,8 @@ struct LogExpression final : Expression {
    *
    * @param lhs Unary operator's operand.
    */
-  explicit LogExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
-    val = std::log(args[0]->val);
-  }
+  explicit constexpr LogExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override { return std::log(x); }
 
@@ -1330,9 +1306,8 @@ struct Log10Expression final : Expression {
    *
    * @param lhs Unary operator's operand.
    */
-  explicit Log10Expression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
-    val = std::log10(args[0]->val);
-  }
+  explicit constexpr Log10Expression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override { return std::log10(x); }
 
@@ -1387,10 +1362,8 @@ struct PowExpression final : Expression {
    * @param lhs Binary operator's left operand.
    * @param rhs Binary operator's right operand.
    */
-  PowExpression(ExpressionPtr lhs, ExpressionPtr rhs)
-      : Expression{std::move(lhs), std::move(rhs)} {
-    val = std::pow(args[0]->val, args[1]->val);
-  }
+  constexpr PowExpression(ExpressionPtr lhs, ExpressionPtr rhs)
+      : Expression{std::move(lhs), std::move(rhs)} {}
 
   double value(double base, double power) const override {
     return std::pow(base, power);
@@ -1489,15 +1462,7 @@ struct SignExpression final : Expression {
    * @param lhs Unary operator's operand.
    */
   explicit constexpr SignExpression(ExpressionPtr lhs)
-      : Expression{std::move(lhs)} {
-    if (args[0]->val < 0.0) {
-      val = -1.0;
-    } else if (args[0]->val == 0.0) {
-      val = 0.0;
-    } else {
-      val = 1.0;
-    }
-  }
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override {
     if (x < 0.0) {
@@ -1552,9 +1517,8 @@ struct SinExpression final : Expression {
    *
    * @param lhs Unary operator's operand.
    */
-  explicit SinExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
-    val = std::sin(args[0]->val);
-  }
+  explicit constexpr SinExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override { return std::sin(x); }
 
@@ -1602,9 +1566,8 @@ struct SinhExpression final : Expression {
    *
    * @param lhs Unary operator's operand.
    */
-  explicit SinhExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
-    val = std::sinh(args[0]->val);
-  }
+  explicit constexpr SinhExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override { return std::sinh(x); }
 
@@ -1652,9 +1615,8 @@ struct SqrtExpression final : Expression {
    *
    * @param lhs Unary operator's operand.
    */
-  explicit SqrtExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
-    val = std::sqrt(args[0]->val);
-  }
+  explicit constexpr SqrtExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override { return std::sqrt(x); }
 
@@ -1704,9 +1666,8 @@ struct TanExpression final : Expression {
    *
    * @param lhs Unary operator's operand.
    */
-  explicit TanExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
-    val = std::tan(args[0]->val);
-  }
+  explicit constexpr TanExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override { return std::tan(x); }
 
@@ -1754,9 +1715,8 @@ struct TanhExpression final : Expression {
    *
    * @param lhs Unary operator's operand.
    */
-  explicit TanhExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
-    val = std::tanh(args[0]->val);
-  }
+  explicit constexpr TanhExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {}
 
   double value(double x, double) const override { return std::tanh(x); }
 

{sleipnirgroup_jormungandr-0.0.1.dev475 → sleipnirgroup_jormungandr-0.0.1.dev477}/include/sleipnir/optimization/solver/exit_status.hpp

@@ -22,21 +22,24 @@ enum class ExitStatus : int8_t {
   TOO_FEW_DOFS = -1,
   /// The solver determined the problem to be locally infeasible and gave up.
   LOCALLY_INFEASIBLE = -2,
+  /// The problem setup frontend determined the problem to have an empty
+  /// feasible region.
+  GLOBALLY_INFEASIBLE = -3,
   /// The linear system factorization failed.
-  FACTORIZATION_FAILED = -3,
+  FACTORIZATION_FAILED = -4,
   /// The backtracking line search failed, and the problem isn't locally
   /// infeasible.
-  LINE_SEARCH_FAILED = -4,
+  LINE_SEARCH_FAILED = -5,
   /// The solver encountered nonfinite initial cost or constraints and gave up.
-  NONFINITE_INITIAL_COST_OR_CONSTRAINTS = -5,
+  NONFINITE_INITIAL_COST_OR_CONSTRAINTS = -6,
   /// The solver encountered diverging primal iterates xₖ and/or sₖ and gave up.
-  DIVERGING_ITERATES = -6,
+  DIVERGING_ITERATES = -7,
   /// The solver returned its solution so far after exceeding the maximum number
   /// of iterations.
-  MAX_ITERATIONS_EXCEEDED = -7,
+  MAX_ITERATIONS_EXCEEDED = -8,
   /// The solver returned its solution so far after exceeding the maximum
   /// elapsed wall clock time.
-  TIMEOUT = -8
+  TIMEOUT = -9,
 };
 
 /**
@@ -57,6 +60,8 @@ SLEIPNIR_DLLEXPORT constexpr std::string_view to_message(
       return "too few degrees of freedom";
     case LOCALLY_INFEASIBLE:
       return "locally infeasible";
+    case GLOBALLY_INFEASIBLE:
+      return "globally infeasible";
     case FACTORIZATION_FAILED:
       return "factorization failed";
     case LINE_SEARCH_FAILED:

{sleipnirgroup_jormungandr-0.0.1.dev475 → sleipnirgroup_jormungandr-0.0.1.dev477}/include/sleipnir/optimization/solver/interior_point.hpp

@@ -223,6 +223,10 @@ SLEIPNIR_DLLEXPORT ExitStatus
 interior_point(const InteriorPointMatrixCallbacks& matrix_callbacks,
                std::span<std::function<bool(const IterationInfo& info)>>
                    iteration_callbacks,
-               const Options& options, Eigen::VectorXd& x);
+               const Options& options,
+#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
+               const Eigen::ArrayX<bool>& bound_constraint_mask,
+#endif
+               Eigen::VectorXd& x);
 
 }  // namespace slp

{sleipnirgroup_jormungandr-0.0.1.dev475 → sleipnirgroup_jormungandr-0.0.1.dev477}/jormungandr/cpp/docstrings.hpp

@@ -78,6 +78,10 @@ up.)doc";
 
 static const char *__doc_slp_ExitStatus_FACTORIZATION_FAILED = R"doc(The linear system factorization failed.)doc";
 
+static const char *__doc_slp_ExitStatus_GLOBALLY_INFEASIBLE =
+R"doc(The problem setup frontend determined the problem to have an empty
+feasible region.)doc";
+
 static const char *__doc_slp_ExitStatus_LINE_SEARCH_FAILED =
 R"doc(The backtracking line search failed, and the problem isn't locally
 infeasible.)doc";

{sleipnirgroup_jormungandr-0.0.1.dev475 → sleipnirgroup_jormungandr-0.0.1.dev477}/jormungandr/cpp/optimization/solver/bind_exit_status.cpp

@@ -17,6 +17,8 @@ void bind_exit_status(nb::enum_<ExitStatus>& e) {
           DOC(slp, ExitStatus, TOO_FEW_DOFS));
   e.value("LOCALLY_INFEASIBLE", ExitStatus::LOCALLY_INFEASIBLE,
           DOC(slp, ExitStatus, LOCALLY_INFEASIBLE));
+  e.value("GLOBALLY_INFEASIBLE", ExitStatus::GLOBALLY_INFEASIBLE,
+          DOC(slp, ExitStatus, GLOBALLY_INFEASIBLE));
   e.value("FACTORIZATION_FAILED", ExitStatus::FACTORIZATION_FAILED,
           DOC(slp, ExitStatus, FACTORIZATION_FAILED));
   e.value("LINE_SEARCH_FAILED", ExitStatus::LINE_SEARCH_FAILED,

{sleipnirgroup_jormungandr-0.0.1.dev475 → sleipnirgroup_jormungandr-0.0.1.dev477}/jormungandr/test/optimization/nonlinear_problem_test.py

@@ -116,6 +116,25 @@ def test_minimum_2d_distance_with_linear_constraint():
     assert y.value() == pytest.approx(2.5, abs=1e-2)
 
 
+def test_conflicting_bounds():
+    problem = Problem()
+
+    x = problem.decision_variable()
+    y = problem.decision_variable()
+
+    problem.minimize(autodiff.hypot(x, y))
+
+    problem.subject_to(autodiff.hypot(x, y) <= 1)
+    problem.subject_to(x >= 0.5)
+    problem.subject_to(x <= -0.5)
+
+    assert problem.cost_function_type() == ExpressionType.NONLINEAR
+    assert problem.equality_constraint_type() == ExpressionType.NONE
+    assert problem.inequality_constraint_type() == ExpressionType.NONLINEAR
+
+    assert problem.solve(diagnostics=True) == ExitStatus.GLOBALLY_INFEASIBLE
+
+
 def test_wachter_and_biegler_line_search_failure():
     # See example 19.2 of [1]
 

{sleipnirgroup_jormungandr-0.0.1.dev475 → sleipnirgroup_jormungandr-0.0.1.dev477}/pyproject.toml

@@ -1,7 +1,7 @@
 [project]
 name = "sleipnirgroup-jormungandr"
 description = "A linearity-exploiting sparse nonlinear constrained optimization problem solver that uses the interior-point method."
-version = "0.0.1.dev475"
+version = "0.0.1.dev477"
 readme = "README.md"
 requires-python = ">=3.9"
 dependencies = [ "matplotlib", "numpy", "scipy" ]

sleipnirgroup_jormungandr-0.0.1.dev477/src/optimization/bounds.hpp

@@ -0,0 +1,220 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <algorithm>
+#include <limits>
+#include <span>
+#include <utility>
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
+#include <gch/small_vector.hpp>
+
+#include "sleipnir/autodiff/expression_type.hpp"
+#include "sleipnir/autodiff/variable.hpp"
+#include "sleipnir/util/assert.hpp"
+
+// See docs/algorithms.md#Works_cited for citation definitions
+
+namespace slp {
+
+struct Bounds {
+  /// Which constraints, if any, are bound constraints.
+  Eigen::ArrayX<bool> bound_constraint_mask;
+
+  /// The tightest bounds on each decision variable.
+  gch::small_vector<std::pair<double, double>> bounds;
+
+  /// Whether or not the constraints are feasible (given previously encountered
+  /// bounds).
+  gch::small_vector<std::pair<Eigen::Index, Eigen::Index>>
+      conflicting_bound_indices;
+};
+
+/**
+ * A "bound constraint" is any linear constraint in one scalar variable.
+ *
+ * Computes which constraints, if any, are bound constraints, the tightest
+ * bounds on each decision variable, and whether or not they're feasible (given
+ * previously encountered bounds),
+ *
+ * @param decision_variables Decision variables corresponding to each column of
+ *   A_i.
+ * @param inequality_constraints Variables representing the left-hand side of
+ *   cᵢ(decision_variables) ≥ 0.
+ * @param A_i The Jacobian of inequality_constraints wrt decision_variables,
+ *   evaluated anywhere, in *row-major* storage; in practice, since we typically
+ *   store Jacobians column-major, the user of this function must perform a
+ *   transpose.
+ */
+inline Bounds get_bounds(
+    std::span<Variable> decision_variables,
+    std::span<Variable> inequality_constraints,
+    const Eigen::SparseMatrix<double, Eigen::RowMajor>& A_i) {
+  // TODO: A blocked, out-of-place transpose should be much faster than
+  // traversing row major on a column major matrix unless we have few linear
+  // constraints (using a heuristic to choose between this and staying column
+  // major based on the number of constraints would be an easy performance
+  // improvement.)
+
+  // NB: Casting to long is unspecified if the size of decision_variable.size()
+  // is greater than the max long value, but then we wouldn't be able to fill
+  // A_i correctly anyway.
+  slp_assert(static_cast<Eigen::Index>(decision_variables.size()) ==
+             A_i.innerSize());
+  slp_assert(static_cast<Eigen::Index>(inequality_constraints.size()) ==
+             A_i.outerSize());
+
+  // Maps each decision variable's index to the indices of its upper and lower
+  // bounds if they exist, or NO_BOUND if they do not; used only for bookkeeping
+  // to compute conflicting bounds
+  constexpr Eigen::Index NO_BOUND = -1;
+  gch::small_vector<std::pair<Eigen::Index, Eigen::Index>>
+      decision_var_indices_to_constraint_indices{decision_variables.size(),
+                                                 {NO_BOUND, NO_BOUND}};
+  // Lists pairs of indices of bound constraints in the inequality constraint
+  // list that conflict with each other
+  gch::small_vector<std::pair<Eigen::Index, Eigen::Index>>
+      conflicting_bound_indices;
+
+  // Maps each decision variable's index to its upper and lower bounds
+  gch::small_vector<std::pair<double, double>> decision_var_indices_to_bounds{
+      decision_variables.size(),
+      {-std::numeric_limits<double>::infinity(),
+       std::numeric_limits<double>::infinity()}};
+
+  // Vector corresponding to the inequality constraints where the i-th element
+  // is 1 if the i-th inequality constraint is a bound and 0 otherwise.
+  Eigen::ArrayX<bool> bound_constraint_mask{inequality_constraints.size()};
+  bound_constraint_mask.fill(false);
+
+  for (decltype(inequality_constraints)::size_type constraint_index = 0;
+       constraint_index < inequality_constraints.size(); ++constraint_index) {
+    // A constraint is a bound iff it is linear and its gradient has a
+    // single nonzero value.
+    if (inequality_constraints[constraint_index].type() !=
+        ExpressionType::LINEAR) {
+      continue;
+    }
+    const Eigen::SparseVector<double>& row_A_i =
+        A_i.innerVector(constraint_index);
+    const auto non_zeros = row_A_i.nonZeros();
+    slp_assert(non_zeros != 0);
+    if (non_zeros > 1) {
+      // Constraint is in more than one variable and therefore not a bound.
+      continue;
+    }
+
+    // Claim: The bound given by a bound constraint is the constraint evaluated
+    // at zero divided by the nonzero element of the constraint's gradient.
+    //
+    // Proof: If c(x) is a bound constraint, then by definition c is a linear
+    // function in one variable, hence there exist a, b ∈ ℝ s.t. c(x) = axᵢ + b
+    // and a ≠ 0. The gradient of c is then aeᵢ (where eᵢ denotes the i-th basis
+    // element), and c(0) = b. If c(x) ≥ 0, then since either a < 0 or a > 0, we
+    // have either x ≤ -b/a or x ≥ -b/a, respectively. ∎
+    Eigen::SparseVector<double>::InnerIterator row_iter(row_A_i);
+    const auto constraint_coefficient =
+        row_iter
+            .value();  // The nonzero value of the j-th constraint's gradient.
+    const auto decision_variable_index = row_iter.index();
+    const auto decision_variable_value =
+        decision_variables[decision_variable_index].value();
+    double constraint_constant;
+    // We need to evaluate this constraint *exactly* at zero.
+    if (decision_variable_value != 0.0) {
+      decision_variables[decision_variable_index].set_value(0.0);
+      constraint_constant = inequality_constraints[constraint_index].value();
+      decision_variables[decision_variable_index].set_value(
+          decision_variable_value);
+    } else {
+      constraint_constant = inequality_constraints[constraint_index].value();
+    }
+
+    // Shouldn't happen since the constraint is supposed to be linear and not a
+    // constant.
+    slp_assert(constraint_coefficient != 0.0);
+
+    // We should always get a finite value when evaluating the constraint at
+    // x = 0 since the constraint is linear.
+    slp_assert(std::isfinite(constraint_constant));
+
+    // This is possible if the user has provided a starting point at which their
+    // problem is ill-defined.
+    slp_assert(std::isfinite(constraint_coefficient));
+
+    // Update bounds; we assume constraints of the form c(x) ≥ 0.
+    auto& [lower_bound, upper_bound] =
+        decision_var_indices_to_bounds[decision_variable_index];
+    auto& [lower_index, upper_index] =
+        decision_var_indices_to_constraint_indices[decision_variable_index];
+    const auto detected_bound = -constraint_constant / constraint_coefficient;
+    if (constraint_coefficient < 0.0 && detected_bound < upper_bound) {
+      upper_bound = detected_bound;
+      upper_index = constraint_index;
+    } else if (constraint_coefficient > 0.0 && detected_bound > lower_bound) {
+      lower_bound = detected_bound;
+      lower_index = constraint_index;
+    }
+
+    // Update conflicting bounds
+    if (lower_bound > upper_bound) {
+      conflicting_bound_indices.emplace_back(lower_index, upper_index);
+    }
+
+    // Set the bound constraint mask appropriately.
+    bound_constraint_mask[constraint_index] = true;
+  }
+  return {bound_constraint_mask, decision_var_indices_to_bounds,
+          conflicting_bound_indices};
+}
+
+/**
+ * Projects the decision variables onto the given bounds, while ensuring some
+ * configurable distance from the boundary if possible. This is designed to
+ * match the algorithm given in section 3.6 of [2].
+ *
+ * @param x A vector of decision variables.
+ * @param decision_variable_indices_to_bounds An array of bounds (stored [lower,
+ *   upper]) for each decision variable in x.
+ * @param κ_1 A constant controlling distance from the lower or upper bound when
+ *   the difference between the upper and lower bound is small.
+ * @param κ_2 A constant controlling distance from the lower or upper bound when
+ *   the difference between the upper and lower bound is large (including when
+ *   one of the bounds is ±∞).
+ */
+template <typename Derived>
+  requires(static_cast<bool>(Eigen::DenseBase<Derived>::IsVectorAtCompileTime))
+inline void project_onto_bounds(
+    Eigen::DenseBase<Derived>& x,
+    std::span<const std::pair<typename Eigen::DenseBase<Derived>::Scalar,
+                              typename Eigen::DenseBase<Derived>::Scalar>>
+        decision_variable_indices_to_bounds,
+    const typename Eigen::DenseBase<Derived>::Scalar κ_1 = 1e-2,
+    const typename Eigen::DenseBase<Derived>::Scalar κ_2 = 1e-2) {
+  slp_assert(κ_1 > 0 && κ_2 > 0 && κ_2 < 0.5);
+
+  Eigen::Index decision_variable_idx = 0;
+  for (const auto& [lower, upper] : decision_variable_indices_to_bounds) {
+    typename Eigen::DenseBase<Derived>::Scalar& x_i =
+        x[decision_variable_idx++];
+
+    // We assume that bound infeasibility is handled elsewhere.
+    slp_assert(lower <= upper);
+
+    // See B.2 in [5] and section 3.6 in [2]
+    if (std::isfinite(lower) && std::isfinite(upper)) {
+      auto p_L =
+          std::min(κ_1 * std::max(1.0, std::abs(lower)), κ_2 * (upper - lower));
+      auto p_U =
+          std::min(κ_1 * std::max(1.0, std::abs(upper)), κ_2 * (upper - lower));
+      x_i = std::min(std::max(lower + p_L, x_i), upper - p_U);
+    } else if (std::isfinite(lower)) {
+      x_i = std::max(x_i, lower + κ_1 * std::max(1.0, std::abs(lower)));
+    } else if (std::isfinite(upper)) {
+      x_i = std::min(x_i, upper - κ_1 * std::max(1.0, std::abs(upper)));
+    }
+  }
+}
+}  // namespace slp

{sleipnirgroup_jormungandr-0.0.1.dev475 → sleipnirgroup_jormungandr-0.0.1.dev477}/src/optimization/problem.cpp

@@ -13,6 +13,7 @@
 #include <Eigen/SparseCore>
 #include <gch/small_vector.hpp>
 
+#include "optimization/bounds.hpp"
 #include "sleipnir/autodiff/expression_type.hpp"
 #include "sleipnir/autodiff/gradient.hpp"
 #include "sleipnir/autodiff/hessian.hpp"
@@ -252,6 +253,19 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
     }
 #endif
 
+    const auto [bound_constraint_mask, bounds, conflicting_bound_indices] =
+        get_bounds(m_decision_variables, m_inequality_constraints, A_i.value());
+    if (!conflicting_bound_indices.empty()) {
+      if (options.diagnostics) {
+        print_bound_constraint_global_infeasibility_error(
+            conflicting_bound_indices);
+      }
+      return ExitStatus::GLOBALLY_INFEASIBLE;
+    }
+
+#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
+    project_onto_bounds(x, bounds);
+#endif
     // Invoke interior-point method solver
     status = interior_point(
         InteriorPointMatrixCallbacks{
@@ -286,7 +300,11 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
               x_ad.set_value(x);
               return A_i.value();
             }},
-        m_iteration_callbacks, options, x);
+        m_iteration_callbacks, options,
+#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
+        bound_constraint_mask,
+#endif
+        x);
   }
 
 #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS

{sleipnirgroup_jormungandr-0.0.1.dev475 → sleipnirgroup_jormungandr-0.0.1.dev477}/src/optimization/solver/interior_point.cpp

@@ -56,7 +56,11 @@ ExitStatus interior_point(
     const InteriorPointMatrixCallbacks& matrix_callbacks,
     std::span<std::function<bool(const IterationInfo& info)>>
         iteration_callbacks,
-    const Options& options, Eigen::VectorXd& x) {
+    const Options& options,
+#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
+    const Eigen::ArrayX<bool>& bound_constraint_mask,
+#endif
+    Eigen::VectorXd& x) {
   const auto solve_start_time = std::chrono::steady_clock::now();
 
   gch::small_vector<SolveProfiler> solve_profilers;
@@ -160,6 +164,10 @@ ExitStatus interior_point(
   Eigen::SparseMatrix<double> A_i = matrices.A_i(x);
 
   Eigen::VectorXd s = Eigen::VectorXd::Ones(num_inequality_constraints);
+#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
+  // We set sʲ = cᵢʲ(x) for each bound inequality constraint index j
+  s = bound_constraint_mask.select(c_i, s);
+#endif
   Eigen::VectorXd y = Eigen::VectorXd::Zero(num_equality_constraints);
   Eigen::VectorXd z = Eigen::VectorXd::Ones(num_inequality_constraints);
 

{sleipnirgroup_jormungandr-0.0.1.dev475 → sleipnirgroup_jormungandr-0.0.1.dev477}/src/util/print_diagnostics.hpp

@@ -149,6 +149,25 @@ inline void print_c_i_local_infeasibility_error(const Eigen::VectorXd& c_i) {
 #define print_c_i_local_infeasibility_error(...)
 #endif
 
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+inline void print_bound_constraint_global_infeasibility_error(
+    const std::span<const std::pair<Eigen::Index, Eigen::Index>>
+        conflicting_lower_upper_bound_indices) {
+  slp::println(
+      "The problem is globally infeasible due to conflicting bound "
+      "constraints:");
+  for (const auto& [lower_bound_idx, upper_bound_idx] :
+       conflicting_lower_upper_bound_indices) {
+    slp::println(
+        "  Inequality constraint {} gives a lower bound that is greater than "
+        "the upper bound given by inequality constraint {}",
+        lower_bound_idx, upper_bound_idx);
+  }
+}
+#else
+#define print_bound_constraint_global_infeasibility_error(...)
+#endif
+
 #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
 /**
  * Prints diagnostics for the current iteration.