OptimizationProblem.hpp

Go to the documentation of this file.

// Copyright (c) Sleipnir contributors
 
#pragma once
 
#include <algorithm>
#include <array>
#include <concepts>
#include <functional>
#include <iterator>
#include <optional>
#include <utility>
 
#include <Eigen/Core>
 
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
#include "sleipnir/optimization/SolverConfig.hpp"
#include "sleipnir/optimization/SolverExitCondition.hpp"
#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"
#include "sleipnir/util/small_vector.hpp"
 
namespace sleipnir {
 
class SLEIPNIR_DLLEXPORT OptimizationProblem {
 public:
  OptimizationProblem() noexcept = default;
 
  [[nodiscard]]
  Variable DecisionVariable() {
    m_decisionVariables.emplace_back();
    return m_decisionVariables.back();
  }
 
  [[nodiscard]]
  VariableMatrix DecisionVariable(int rows, int cols = 1) {
    m_decisionVariables.reserve(m_decisionVariables.size() + rows * cols);
 
    VariableMatrix vars{rows, cols};
 
    for (int row = 0; row < rows; ++row) {
      for (int col = 0; col < cols; ++col) {
        m_decisionVariables.emplace_back();
        vars(row, col) = m_decisionVariables.back();
      }
    }
 
    return vars;
  }
 
  [[nodiscard]]
  VariableMatrix SymmetricDecisionVariable(int rows) {
    // We only need to store the lower triangle of an n x n symmetric matrix;
    // the other elements are duplicates. The lower triangle has (n² + n)/2
    // elements.
    //
    //   n
    //   Σ k = (n² + n)/2
    //  k=1
    m_decisionVariables.reserve(m_decisionVariables.size() +
                                (rows * rows + rows) / 2);
 
    VariableMatrix vars{rows, rows};
 
    for (int row = 0; row < rows; ++row) {
      for (int col = 0; col <= row; ++col) {
        m_decisionVariables.emplace_back();
        vars(row, col) = m_decisionVariables.back();
        vars(col, row) = m_decisionVariables.back();
      }
    }
 
    return vars;
  }
 
  void Minimize(const Variable& cost) {
    m_f = cost;
    status.costFunctionType = m_f.value().Type();
  }
 
  void Minimize(Variable&& cost) {
    m_f = std::move(cost);
    status.costFunctionType = m_f.value().Type();
  }
 
  void Maximize(const Variable& objective) {
    // Maximizing a cost function is the same as minimizing its negative
    m_f = -objective;
    status.costFunctionType = m_f.value().Type();
  }
 
  void Maximize(Variable&& objective) {
    // Maximizing a cost function is the same as minimizing its negative
    m_f = -std::move(objective);
    status.costFunctionType = m_f.value().Type();
  }
 
  void SubjectTo(const EqualityConstraints& constraint) {
    // Get the highest order equality constraint expression type
    for (const auto& c : constraint.constraints) {
      status.equalityConstraintType =
          std::max(status.equalityConstraintType, c.Type());
    }
 
    m_equalityConstraints.reserve(m_equalityConstraints.size() +
                                  constraint.constraints.size());
    std::copy(constraint.constraints.begin(), constraint.constraints.end(),
              std::back_inserter(m_equalityConstraints));
  }
 
  void SubjectTo(EqualityConstraints&& constraint) {
    // Get the highest order equality constraint expression type
    for (const auto& c : constraint.constraints) {
      status.equalityConstraintType =
          std::max(status.equalityConstraintType, c.Type());
    }
 
    m_equalityConstraints.reserve(m_equalityConstraints.size() +
                                  constraint.constraints.size());
    std::copy(constraint.constraints.begin(), constraint.constraints.end(),
              std::back_inserter(m_equalityConstraints));
  }
 
  void SubjectTo(const InequalityConstraints& constraint) {
    // Get the highest order inequality constraint expression type
    for (const auto& c : constraint.constraints) {
      status.inequalityConstraintType =
          std::max(status.inequalityConstraintType, c.Type());
    }
 
    m_inequalityConstraints.reserve(m_inequalityConstraints.size() +
                                    constraint.constraints.size());
    std::copy(constraint.constraints.begin(), constraint.constraints.end(),
              std::back_inserter(m_inequalityConstraints));
  }
 
  void SubjectTo(InequalityConstraints&& constraint) {
    // Get the highest order inequality constraint expression type
    for (const auto& c : constraint.constraints) {
      status.inequalityConstraintType =
          std::max(status.inequalityConstraintType, c.Type());
    }
 
    m_inequalityConstraints.reserve(m_inequalityConstraints.size() +
                                    constraint.constraints.size());
    std::copy(constraint.constraints.begin(), constraint.constraints.end(),
              std::back_inserter(m_inequalityConstraints));
  }
 
  SolverStatus Solve(const SolverConfig& config = SolverConfig{}) {
    // Create the initial value column vector
    Eigen::VectorXd x{m_decisionVariables.size()};
    for (size_t i = 0; i < m_decisionVariables.size(); ++i) {
      x(i) = m_decisionVariables[i].Value();
    }
 
    status.exitCondition = SolverExitCondition::kSuccess;
 
    // If there's no cost function, make it zero and continue
    if (!m_f.has_value()) {
      m_f = Variable();
    }
 
    if (config.diagnostics) {
      constexpr std::array kExprTypeToName{"empty", "constant", "linear",
                                           "quadratic", "nonlinear"};
 
      // Print cost function and constraint expression types
      sleipnir::println(
          "The cost function is {}.",
          kExprTypeToName[static_cast<int>(status.costFunctionType)]);
      sleipnir::println(
          "The equality constraints are {}.",
          kExprTypeToName[static_cast<int>(status.equalityConstraintType)]);
      sleipnir::println(
          "The inequality constraints are {}.",
          kExprTypeToName[static_cast<int>(status.inequalityConstraintType)]);
      sleipnir::println("");
 
      // Print problem dimensionality
      sleipnir::println("Number of decision variables: {}",
                        m_decisionVariables.size());
      sleipnir::println("Number of equality constraints: {}",
                        m_equalityConstraints.size());
      sleipnir::println("Number of inequality constraints: {}\n",
                        m_inequalityConstraints.size());
    }
 
    // If the problem is empty or constant, there's nothing to do
    if (status.costFunctionType <= ExpressionType::kConstant &&
        status.equalityConstraintType <= ExpressionType::kConstant &&
        status.inequalityConstraintType <= ExpressionType::kConstant) {
      return status;
    }
 
    // Solve the optimization problem
    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 {
      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));
    }
 
    // Assign the solution to the original Variable instances
    VariableMatrix{m_decisionVariables}.SetValue(x);
 
    return status;
  }
 
  template <typename F>
    requires requires(F callback, const SolverIterationInfo& info) {
      { callback(info) } -> std::same_as<void>;
    }
  void Callback(F&& callback) {
    m_callback = [=, callback = std::forward<F>(callback)](
                     const SolverIterationInfo& info) {
      callback(info);
      return false;
    };
  }
 
  template <typename F>
    requires requires(F callback, const SolverIterationInfo& info) {
      { callback(info) } -> std::same_as<bool>;
    }
  void Callback(F&& callback) {
    m_callback = std::forward<F>(callback);
  }
 
 private:
  // The list of decision variables, which are the root of the problem's
  // expression tree
  small_vector<Variable> m_decisionVariables;
 
  // The cost function: f(x)
  std::optional<Variable> m_f;
 
  // The list of equality constraints: cₑ(x) = 0
  small_vector<Variable> m_equalityConstraints;
 
  // The list of inequality constraints: cᵢ(x) ≥ 0
  small_vector<Variable> m_inequalityConstraints;
 
  // The user callback
  std::function<bool(const SolverIterationInfo& info)> m_callback =
      [](const SolverIterationInfo&) { return false; };
 
  // The solver status
  SolverStatus status;
};
 
}  // namespace sleipnir