17#include <Eigen/SparseCore>
28#include "sleipnir/optimization/solver/interior_point.hpp"
30#include "sleipnir/optimization/solver/newton.hpp"
32#include "sleipnir/optimization/solver/sqp.hpp"
66template <
typename Scalar>
77 m_decision_variables.emplace_back();
78 return m_decision_variables.back();
88 m_decision_variables.reserve(m_decision_variables.size() + rows * cols);
92 for (
int row = 0; row < rows; ++row) {
93 for (
int col = 0; col < cols; ++col) {
94 m_decision_variables.emplace_back();
95 vars(row, col) = m_decision_variables.back();
120 m_decision_variables.reserve(m_decision_variables.size() +
121 (rows * rows + rows) / 2);
125 for (
int row = 0; row < rows; ++row) {
126 for (
int col = 0; col <= row; ++col) {
127 m_decision_variables.emplace_back();
128 vars(row, col) = m_decision_variables.back();
129 vars(col, row) = m_decision_variables.back();
175 m_f = -std::move(objective);
183 m_equality_constraints.reserve(m_equality_constraints.size() +
186 std::back_inserter(m_equality_constraints));
194 m_equality_constraints.reserve(m_equality_constraints.size() +
195 constraint.constraints.size());
196 std::ranges::copy(constraint.constraints,
197 std::back_inserter(m_equality_constraints));
205 m_inequality_constraints.reserve(m_inequality_constraints.size() +
208 std::back_inserter(m_inequality_constraints));
216 m_inequality_constraints.reserve(m_inequality_constraints.size() +
217 constraint.constraints.size());
218 std::ranges::copy(constraint.constraints,
219 std::back_inserter(m_inequality_constraints));
227 return m_f.value().type();
237 if (!m_equality_constraints.empty()) {
238 return std::ranges::max(m_equality_constraints, {},
250 if (!m_inequality_constraints.empty()) {
251 return std::ranges::max(m_inequality_constraints, {},
268 [[maybe_unused]]
bool spy =
false) {
269 using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
270 using SparseMatrix = Eigen::SparseMatrix<Scalar>;
271 using SparseVector = Eigen::SparseVector<Scalar>;
274 DenseVector x{m_decision_variables.size()};
275 for (
size_t i = 0; i < m_decision_variables.size(); ++i) {
276 x[i] = m_decision_variables[i].value();
279 if (options.diagnostics) {
280 print_exit_conditions(options);
281 print_problem_analysis();
293#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
294 if (options.diagnostics) {
302 ad_setup_profilers.emplace_back(
"setup").start();
304 VariableMatrix<Scalar> x_ad{m_decision_variables};
307 Variable f = m_f.value_or(Scalar(0));
310 ad_setup_profilers.emplace_back(
" ↳ ∇f(x)").start();
312 ad_setup_profilers.back().stop();
315 int num_decision_variables = m_decision_variables.size();
317 int num_equality_constraints = m_equality_constraints.size();
319 int num_inequality_constraints = m_inequality_constraints.size();
323 for (
const auto& callback : m_iteration_callbacks) {
324 callbacks.emplace_back(callback);
326 for (
const auto& callback : m_persistent_iteration_callbacks) {
327 callbacks.emplace_back(callback);
332 if (m_equality_constraints.empty() && m_inequality_constraints.empty()) {
333 if (options.diagnostics) {
338 ad_setup_profilers.emplace_back(
" ↳ ∇²ₓₓL").start();
339 Hessian<Scalar, Eigen::Lower> H{f, x_ad};
340 ad_setup_profilers.back().stop();
342 ad_setup_profilers[0].stop();
344#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
346 std::unique_ptr<Spy<Scalar>> H_spy;
349 H_spy = std::make_unique<Spy<Scalar>>(
350 "H.spy",
"Hessian",
"Decision variables",
"Decision variables",
351 num_decision_variables, num_decision_variables);
352 callbacks.push_back([&](
const IterationInfo<Scalar>& info) ->
bool {
360 status = newton<Scalar>(NewtonMatrixCallbacks<Scalar>{
361 [&](
const DenseVector& x) -> Scalar {
365 [&](
const DenseVector& x) -> SparseVector {
369 [&](
const DenseVector& x) -> SparseMatrix {
373 callbacks, options, x);
374 }
else if (m_inequality_constraints.empty()) {
375 if (options.diagnostics) {
379 VariableMatrix<Scalar> c_e_ad{m_equality_constraints};
382 ad_setup_profilers.emplace_back(
" ↳ ∂cₑ/∂x").start();
383 Jacobian A_e{c_e_ad, x_ad};
384 ad_setup_profilers.back().stop();
387 VariableMatrix<Scalar> y_ad(num_equality_constraints);
391 ad_setup_profilers.emplace_back(
" ↳ ∇²ₓₓL").start();
392 Hessian<Scalar, Eigen::Lower> H{L, x_ad};
393 ad_setup_profilers.back().stop();
395 ad_setup_profilers[0].stop();
397#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
399 std::unique_ptr<Spy<Scalar>> H_spy;
400 std::unique_ptr<Spy<Scalar>> A_e_spy;
403 H_spy = std::make_unique<Spy<Scalar>>(
404 "H.spy",
"Hessian",
"Decision variables",
"Decision variables",
405 num_decision_variables, num_decision_variables);
406 A_e_spy = std::make_unique<Spy<Scalar>>(
407 "A_e.spy",
"Equality constraint Jacobian",
"Constraints",
408 "Decision variables", num_equality_constraints,
409 num_decision_variables);
410 callbacks.push_back([&](
const IterationInfo<Scalar>& info) ->
bool {
412 A_e_spy->add(info.A_e);
419 status = sqp<Scalar>(
420 SQPMatrixCallbacks<Scalar>{
421 [&](
const DenseVector& x) -> Scalar {
425 [&](
const DenseVector& x) -> SparseVector {
429 [&](
const DenseVector& x,
const DenseVector& y) -> SparseMatrix {
434 [&](
const DenseVector& x) -> DenseVector {
436 return c_e_ad.value();
438 [&](
const DenseVector& x) -> SparseMatrix {
442 callbacks, options, x);
444 if (options.diagnostics) {
448 VariableMatrix<Scalar> c_e_ad{m_equality_constraints};
449 VariableMatrix<Scalar> c_i_ad{m_inequality_constraints};
452 ad_setup_profilers.emplace_back(
" ↳ ∂cₑ/∂x").start();
453 Jacobian A_e{c_e_ad, x_ad};
454 ad_setup_profilers.back().stop();
457 ad_setup_profilers.emplace_back(
" ↳ ∂cᵢ/∂x").start();
458 Jacobian A_i{c_i_ad, x_ad};
459 ad_setup_profilers.back().stop();
462 VariableMatrix<Scalar> y_ad(num_equality_constraints);
463 VariableMatrix<Scalar> z_ad(num_inequality_constraints);
464 Variable L = f - y_ad.T() * c_e_ad - z_ad.T() * c_i_ad;
467 ad_setup_profilers.emplace_back(
" ↳ ∇²ₓₓL").start();
468 Hessian<Scalar, Eigen::Lower> H{L, x_ad};
469 ad_setup_profilers.back().stop();
471 ad_setup_profilers[0].stop();
473#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
475 std::unique_ptr<Spy<Scalar>> H_spy;
476 std::unique_ptr<Spy<Scalar>> A_e_spy;
477 std::unique_ptr<Spy<Scalar>> A_i_spy;
480 H_spy = std::make_unique<Spy<Scalar>>(
481 "H.spy",
"Hessian",
"Decision variables",
"Decision variables",
482 num_decision_variables, num_decision_variables);
483 A_e_spy = std::make_unique<Spy<Scalar>>(
484 "A_e.spy",
"Equality constraint Jacobian",
"Constraints",
485 "Decision variables", num_equality_constraints,
486 num_decision_variables);
487 A_i_spy = std::make_unique<Spy<Scalar>>(
488 "A_i.spy",
"Inequality constraint Jacobian",
"Constraints",
489 "Decision variables", num_inequality_constraints,
490 num_decision_variables);
491 callbacks.push_back([&](
const IterationInfo<Scalar>& info) ->
bool {
493 A_e_spy->add(info.A_e);
494 A_i_spy->add(info.A_i);
500 const auto [bound_constraint_mask,
bounds, conflicting_bound_indices] =
503 if (!conflicting_bound_indices.empty()) {
504 if (options.diagnostics) {
506 conflicting_bound_indices);
511#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
515 status = interior_point<Scalar>(
516 InteriorPointMatrixCallbacks<Scalar>{
517 [&](
const DenseVector& x) -> Scalar {
521 [&](
const DenseVector& x) -> SparseVector {
525 [&](
const DenseVector& x,
const DenseVector& y,
526 const DenseVector& z) -> SparseMatrix {
532 [&](
const DenseVector& x) -> DenseVector {
534 return c_e_ad.value();
536 [&](
const DenseVector& x) -> SparseMatrix {
540 [&](
const DenseVector& x) -> DenseVector {
542 return c_i_ad.value();
544 [&](
const DenseVector& x) -> SparseMatrix {
549#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
550 bound_constraint_mask,
555 if (options.diagnostics) {
561 VariableMatrix<Scalar>{m_decision_variables}.set_value(x);
571 template <
typename F>
572 requires requires(F callback,
const IterationInfo<Scalar>& info) {
573 { callback(info) } -> std::same_as<void>;
576 m_iteration_callbacks.emplace_back(
590 template <
typename F>
592 { callback(info) } -> std::same_as<bool>;
595 m_iteration_callbacks.emplace_back(std::forward<F>(callback));
609 template <
typename F>
611 { callback(info) } -> std::same_as<bool>;
614 m_persistent_iteration_callbacks.emplace_back(std::forward<F>(callback));
623 std::optional<Variable<Scalar>> m_f;
633 m_iteration_callbacks;
635 m_persistent_iteration_callbacks;
637 void print_exit_conditions([[maybe_unused]]
const Options& options) {
641 if (!m_iteration_callbacks.empty() ||
642 !m_persistent_iteration_callbacks.empty()) {
645 if (std::isfinite(options.max_iterations)) {
646 slp::println(
" ↳ executed {} iterations", options.max_iterations);
648 if (std::isfinite(options.timeout.count())) {
653 void print_problem_analysis() {
654 constexpr std::array types{
"no",
"constant",
"linear",
"quadratic",
666 if (m_decision_variables.size() == 1) {
669 slp::print(
"\n{} decision variables\n", m_decision_variables.size());
672 auto print_constraint_types =
674 std::array<size_t, 5> counts{};
675 for (
const auto& constraint : constraints) {
676 ++counts[
static_cast<uint8_t
>(constraint.type())];
678 for (
size_t i = 0; i < counts.size(); ++i) {
679 constexpr std::array
names{
"empty",
"constant",
"linear",
680 "quadratic",
"nonlinear"};
681 const auto&
count = counts[i];
690 if (m_equality_constraints.size() == 1) {
693 slp::println(
"{} equality constraints", m_equality_constraints.size());
695 print_constraint_types(m_equality_constraints);
696 if (m_inequality_constraints.size() == 1) {
700 m_inequality_constraints.size());
702 print_constraint_types(m_inequality_constraints);
#define EXPORT_TEMPLATE_DECLARE(export)
Definition SymbolExports.hpp:94
within a display generated by the Derivative if and wherever such third party notices normally appear The contents of the NOTICE file are for informational purposes only and do not modify the License You may add Your own attribution notices within Derivative Works that You alongside or as an addendum to the NOTICE text from the provided that such additional attribution notices cannot be construed as modifying the License You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for or distribution of Your or for any such Derivative Works as a provided Your and distribution of the Work otherwise complies with the conditions stated in this License Submission of Contributions Unless You explicitly state any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this without any additional terms or conditions Notwithstanding the nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions Trademarks This License does not grant permission to use the trade names
Definition ThirdPartyNotices.txt:234
@ name
Definition base.h:690
This class allows the user to pose a constrained nonlinear optimization problem in natural mathematic...
Definition problem.hpp:67
VariableMatrix< Scalar > symmetric_decision_variable(int rows)
Create a symmetric matrix of decision variables in the optimization problem.
Definition problem.hpp:112
void subject_to(InequalityConstraints< Scalar > &&constraint)
Tells the solver to solve the problem while satisfying the given inequality constraint.
Definition problem.hpp:215
void add_callback(F &&callback)
Adds a callback to be called at the beginning of each solver iteration.
Definition problem.hpp:594
void subject_to(EqualityConstraints< Scalar > &&constraint)
Tells the solver to solve the problem while satisfying the given equality constraint.
Definition problem.hpp:193
ExpressionType equality_constraint_type() const
Returns the type of the highest order equality constraint.
Definition problem.hpp:236
void maximize(Variable< Scalar > &&objective)
Tells the solver to maximize the output of the given objective function.
Definition problem.hpp:173
void add_persistent_callback(F &&callback)
Adds a callback to be called at the beginning of each solver iteration.
Definition problem.hpp:613
void subject_to(const InequalityConstraints< Scalar > &constraint)
Tells the solver to solve the problem while satisfying the given inequality constraint.
Definition problem.hpp:204
ExpressionType inequality_constraint_type() const
Returns the type of the highest order inequality constraint.
Definition problem.hpp:249
void minimize(const Variable< Scalar > &cost)
Tells the solver to minimize the output of the given cost function.
Definition problem.hpp:143
Problem() noexcept=default
Construct the optimization problem.
VariableMatrix< Scalar > decision_variable(int rows, int cols=1)
Create a matrix of decision variables in the optimization problem.
Definition problem.hpp:87
void minimize(Variable< Scalar > &&cost)
Tells the solver to minimize the output of the given cost function.
Definition problem.hpp:152
void clear_callbacks()
Clears the registered callbacks.
Definition problem.hpp:599
void subject_to(const EqualityConstraints< Scalar > &constraint)
Tells the solver to solve the problem while satisfying the given equality constraint.
Definition problem.hpp:182
void maximize(const Variable< Scalar > &objective)
Tells the solver to maximize the output of the given objective function.
Definition problem.hpp:161
void add_callback(F &&callback)
Adds a callback to be called at the beginning of each solver iteration.
Definition problem.hpp:575
ExpressionType cost_function_type() const
Returns the cost function's type.
Definition problem.hpp:225
ExitStatus solve(const Options &options=Options{}, bool spy=false)
Solve the optimization problem.
Definition problem.hpp:267
Variable< Scalar > decision_variable()
Create a decision variable in the optimization problem.
Definition problem.hpp:76
An autodiff variable pointing to an expression node.
Definition variable.hpp:47
ExpressionType type() const
Returns the type of this expression (constant, linear, quadratic, or nonlinear).
Definition variable.hpp:149
A matrix of autodiff variables.
Definition variable_matrix.hpp:33
constexpr auto count() -> int
Definition base.h:1081
wpi::util::SmallVector< T > small_vector
Definition small_vector.hpp:10
static constexpr empty_t empty
Designates an uninitialized VariableMatrix.
Definition empty.hpp:11
Definition expression_graph.hpp:11
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=typename Eigen::DenseBase< Derived >::Scalar(1e-2), const typename Eigen::DenseBase< Derived >::Scalar κ_2=typename Eigen::DenseBase< Derived >::Scalar(1e-2))
Projects the decision variables onto the given bounds, while ensuring some configurable distance from...
Definition bounds.hpp:195
ExpressionType
Expression type.
Definition expression_type.hpp:16
@ CONSTANT
The expression is a constant.
Definition expression_type.hpp:20
@ NONE
There is no expression.
Definition expression_type.hpp:18
ExitStatus
Solver exit status. Negative values indicate failure.
Definition exit_status.hpp:14
@ GLOBALLY_INFEASIBLE
The problem setup frontend determined the problem to have an empty feasible region.
Definition exit_status.hpp:25
@ SUCCESS
Solved the problem to the desired tolerance.
Definition exit_status.hpp:16
auto bounds(L &&l, X &&x, U &&u)
Helper function for creating bound constraints.
Definition variable.hpp:910
void print_bound_constraint_global_infeasibility_error(const std::span< const std::pair< Eigen::Index, Eigen::Index > > conflicting_lower_upper_bound_indices)
Definition print_diagnostics.hpp:153
Variable(T< Scalar >) -> Variable< Scalar >
void println(fmt::format_string< T... > fmt, T &&... args)
Wrapper around fmt::println() that squelches write failure exceptions.
Definition print.hpp:37
Bounds< Scalar > get_bounds(std::span< Variable< Scalar > > decision_variables, std::span< Variable< Scalar > > inequality_constraints, const Eigen::SparseMatrix< Scalar, Eigen::RowMajor > &A_i)
A "bound constraint" is any linear constraint in one scalar variable.
Definition bounds.hpp:55
void print(fmt::format_string< T... > fmt, T &&... args)
Wrapper around fmt::print() that squelches write failure exceptions.
Definition print.hpp:19
void print_autodiff_diagnostics(const gch::small_vector< SetupProfiler > &setup_profilers)
Prints autodiff diagnostics.
Definition print_diagnostics.hpp:323
A vector of equality constraints of the form cₑ(x) = 0.
Definition variable.hpp:683
gch::small_vector< Variable< Scalar > > constraints
A vector of scalar equality constraints.
Definition variable.hpp:685
A vector of inequality constraints of the form cᵢ(x) ≥ 0.
Definition variable.hpp:740
gch::small_vector< Variable< Scalar > > constraints
A vector of scalar inequality constraints.
Definition variable.hpp:742
Solver iteration information exposed to an iteration callback.
Definition iteration_info.hpp:14
Solver options.
Definition options.hpp:13
#define SLEIPNIR_DLLEXPORT
Definition symbol_exports.hpp:34