287 using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
288 using SparseMatrix = Eigen::SparseMatrix<Scalar>;
289 using SparseVector = Eigen::SparseVector<Scalar>;
292 DenseVector x{m_decision_variables.size()};
293 for (
size_t i = 0; i < m_decision_variables.size(); ++i) {
294 x[i] = m_decision_variables[i].value();
297 if (options.diagnostics) {
298 print_exit_conditions(options);
299 print_problem_analysis();
308 if (f_type <= ExpressionType::CONSTANT &&
309 c_e_type <= ExpressionType::CONSTANT &&
310 c_i_type <= ExpressionType::CONSTANT) {
311#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
312 if (options.diagnostics) {
313 slp::println(
"\nInvoking no-op solver\n");
316 return ExitStatus::SUCCESS;
319 VariableMatrix<Scalar> x_ad{m_decision_variables};
322 Variable f = m_f.value_or(Scalar(0));
324 int num_decision_variables = m_decision_variables.size();
325 int num_equality_constraints = m_equality_constraints.size();
326 int num_inequality_constraints = m_inequality_constraints.size();
328 gch::small_vector<std::function<bool(
const IterationInfo<Scalar>& info)>>
330 for (
const auto& callback : m_iteration_callbacks) {
331 iteration_callbacks.emplace_back(callback);
333 for (
const auto& callback : m_persistent_iteration_callbacks) {
334 iteration_callbacks.emplace_back(callback);
339 if (m_equality_constraints.empty() && m_inequality_constraints.empty()) {
340 if (options.diagnostics) {
341 slp::println(
"\nInvoking Newton solver\n");
344 gch::small_vector<SetupProfiler> ad_setup_profilers;
345 ad_setup_profilers.emplace_back(
"setup");
346 ad_setup_profilers.emplace_back(
"↳ ∇f(x)");
347 ad_setup_profilers.emplace_back(
"↳ ∇²ₓₓL");
349 ad_setup_profilers[0].start();
352 ad_setup_profilers[1].start();
354 ad_setup_profilers[1].stop();
357 ad_setup_profilers[2].start();
358 Hessian<Scalar, Eigen::Lower> H{f, x_ad};
359 ad_setup_profilers[2].stop();
361 ad_setup_profilers[0].stop();
363 if (options.diagnostics) {
364 print_setup_diagnostics(ad_setup_profilers);
367#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
369 std::unique_ptr<Spy<Scalar>> H_spy;
372 H_spy = std::make_unique<Spy<Scalar>>(
373 "H.spy",
"Hessian",
"Decision variables",
"Decision variables",
374 num_decision_variables, num_decision_variables);
375 iteration_callbacks.push_back(
376 [&](
const IterationInfo<Scalar>& info) ->
bool {
386 const ProblemScaling<Scalar> scaling{g.value()};
388 NewtonMatrixCallbacks<Scalar> matrix_callbacks{
389 num_decision_variables,
390 [&](
const DenseVector& x) -> Scalar {
392 return scaling.f * f.value();
394 [&](
const DenseVector& x) -> SparseVector {
396 return scaling.f * g.value();
398 [&](
const DenseVector& x) -> SparseMatrix {
400 return scaling.f * H.value();
406 newton<Scalar>(matrix_callbacks, iteration_callbacks, options, x);
407 }
else if (m_inequality_constraints.empty()) {
408 if (options.diagnostics) {
409 slp::println(
"\nInvoking SQP solver\n");
412 VariableMatrix<Scalar> c_e_ad{m_equality_constraints};
413 VariableMatrix<Scalar> y_ad(num_equality_constraints);
415 gch::small_vector<SetupProfiler> ad_setup_profilers;
416 ad_setup_profilers.emplace_back(
"setup");
417 ad_setup_profilers.emplace_back(
"↳ ∇f(x)");
418 ad_setup_profilers.emplace_back(
"↳ ∇²ₓₓL");
419 ad_setup_profilers.emplace_back(
" ↳ ∇²ₓₓL_f");
420 ad_setup_profilers.emplace_back(
" ↳ ∇²ₓₓL_c");
421 ad_setup_profilers.emplace_back(
"↳ ∂cₑ/∂x");
423 ad_setup_profilers[0].start();
426 ad_setup_profilers[1].start();
428 ad_setup_profilers[1].stop();
430 ad_setup_profilers[2].start();
433 ad_setup_profilers[3].start();
434 Hessian<Scalar, Eigen::Lower> H_f{f, x_ad};
435 ad_setup_profilers[3].stop();
438 ad_setup_profilers[4].start();
439 Hessian<Scalar, Eigen::Lower> H_c{-y_ad.T() * c_e_ad, x_ad};
440 ad_setup_profilers[4].stop();
442 ad_setup_profilers[2].stop();
445 ad_setup_profilers[5].start();
446 Jacobian A_e{c_e_ad, x_ad};
447 ad_setup_profilers[5].stop();
449 ad_setup_profilers[0].stop();
451 if (options.diagnostics) {
452 print_setup_diagnostics(ad_setup_profilers);
455#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
457 std::unique_ptr<Spy<Scalar>> H_spy;
458 std::unique_ptr<Spy<Scalar>> A_e_spy;
461 H_spy = std::make_unique<Spy<Scalar>>(
462 "H.spy",
"Hessian",
"Decision variables",
"Decision variables",
463 num_decision_variables, num_decision_variables);
464 A_e_spy = std::make_unique<Spy<Scalar>>(
465 "A_e.spy",
"Equality constraint Jacobian",
"Constraints",
466 "Decision variables", num_equality_constraints,
467 num_decision_variables);
468 iteration_callbacks.push_back(
469 [&](
const IterationInfo<Scalar>& info) ->
bool {
471 A_e_spy->add(info.A_e);
481 const ProblemScaling<Scalar> scaling{g.value(), A_e.value()};
483 SQPMatrixCallbacks<Scalar> matrix_callbacks{
484 num_decision_variables,
485 num_equality_constraints,
486 [&](
const DenseVector& x) -> Scalar {
488 return scaling.f * f.value();
490 [&](
const DenseVector& x) -> SparseVector {
492 return scaling.f * g.value();
494 [&](
const DenseVector& x,
const DenseVector& y) -> SparseMatrix {
496 y_ad.set_value(scaling.c_e.cwiseProduct(y));
497 return scaling.f * H_f.value() + H_c.value();
499 [&](
const DenseVector& x,
const DenseVector& y) -> SparseMatrix {
501 y_ad.set_value(scaling.c_e.cwiseProduct(y));
504 [&](
const DenseVector& x) -> DenseVector {
506 return scaling.c_e.cwiseProduct(c_e_ad.value());
508 [&](
const DenseVector& x) -> SparseMatrix {
510 return scaling.c_e.asDiagonal() * A_e.value();
515 status = sqp<Scalar>(matrix_callbacks, iteration_callbacks, options, x);
517 if (options.diagnostics) {
518 slp::println(
"\nInvoking IPM solver\n");
521 VariableMatrix<Scalar> c_e_ad{m_equality_constraints};
522 VariableMatrix<Scalar> c_i_ad{m_inequality_constraints};
523 VariableMatrix<Scalar> y_ad(num_equality_constraints);
524 VariableMatrix<Scalar> z_ad(num_inequality_constraints);
526 gch::small_vector<SetupProfiler> ad_setup_profilers;
527 ad_setup_profilers.emplace_back(
"setup");
528 ad_setup_profilers.emplace_back(
"↳ ∇f(x)");
529 ad_setup_profilers.emplace_back(
"↳ ∇²ₓₓL");
530 ad_setup_profilers.emplace_back(
" ↳ ∇²ₓₓL_f");
531 ad_setup_profilers.emplace_back(
" ↳ ∇²ₓₓL_c");
532 ad_setup_profilers.emplace_back(
"↳ ∂cₑ/∂x");
533 ad_setup_profilers.emplace_back(
"↳ ∂cᵢ/∂x");
535 ad_setup_profilers[0].start();
538 ad_setup_profilers[1].start();
540 ad_setup_profilers[1].stop();
542 ad_setup_profilers[2].start();
545 ad_setup_profilers[3].start();
546 Hessian<Scalar, Eigen::Lower> H_f{f, x_ad};
547 ad_setup_profilers[3].stop();
550 ad_setup_profilers[4].start();
551 Hessian<Scalar, Eigen::Lower> H_c{-y_ad.T() * c_e_ad - z_ad.T() * c_i_ad,
553 ad_setup_profilers[4].stop();
555 ad_setup_profilers[2].stop();
558 ad_setup_profilers[5].start();
559 Jacobian A_e{c_e_ad, x_ad};
560 ad_setup_profilers[5].stop();
563 ad_setup_profilers[6].start();
564 Jacobian A_i{c_i_ad, x_ad};
565 ad_setup_profilers[6].stop();
567 ad_setup_profilers[0].stop();
569 if (options.diagnostics) {
570 print_setup_diagnostics(ad_setup_profilers);
573#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
575 std::unique_ptr<Spy<Scalar>> H_spy;
576 std::unique_ptr<Spy<Scalar>> A_e_spy;
577 std::unique_ptr<Spy<Scalar>> A_i_spy;
580 H_spy = std::make_unique<Spy<Scalar>>(
581 "H.spy",
"Hessian",
"Decision variables",
"Decision variables",
582 num_decision_variables, num_decision_variables);
583 A_e_spy = std::make_unique<Spy<Scalar>>(
584 "A_e.spy",
"Equality constraint Jacobian",
"Constraints",
585 "Decision variables", num_equality_constraints,
586 num_decision_variables);
587 A_i_spy = std::make_unique<Spy<Scalar>>(
588 "A_i.spy",
"Inequality constraint Jacobian",
"Constraints",
589 "Decision variables", num_inequality_constraints,
590 num_decision_variables);
591 iteration_callbacks.push_back(
592 [&](
const IterationInfo<Scalar>& info) ->
bool {
594 A_e_spy->add(info.A_e);
595 A_i_spy->add(info.A_i);
601 const auto [bound_constraint_mask, bounds, conflicting_bound_indices] =
602 get_bounds<Scalar>(m_decision_variables, m_inequality_constraints,
604 if (!conflicting_bound_indices.empty()) {
605 if (options.diagnostics) {
606 print_bound_constraint_global_infeasibility_error(
607 conflicting_bound_indices);
609 return ExitStatus::GLOBALLY_INFEASIBLE;
612#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
613 project_onto_bounds(x, bounds);
620 const ProblemScaling<Scalar> scaling{g.value(), A_e.value(), A_i.value()};
622 InteriorPointMatrixCallbacks<Scalar> matrix_callbacks{
623 num_decision_variables,
624 num_equality_constraints,
625 num_inequality_constraints,
626 [&](
const DenseVector& x) -> Scalar {
628 return scaling.f * f.value();
630 [&](
const DenseVector& x) -> SparseVector {
632 return scaling.f * g.value();
634 [&](
const DenseVector& x,
const DenseVector& y,
635 const DenseVector& z) -> SparseMatrix {
637 y_ad.set_value(scaling.c_e.cwiseProduct(y));
638 z_ad.set_value(scaling.c_i.cwiseProduct(z));
639 return scaling.f * H_f.value() + H_c.value();
641 [&](
const DenseVector& x,
const DenseVector& y,
642 const DenseVector& z) -> SparseMatrix {
644 y_ad.set_value(scaling.c_e.cwiseProduct(y));
645 z_ad.set_value(scaling.c_i.cwiseProduct(z));
648 [&](
const DenseVector& x) -> DenseVector {
650 return scaling.c_e.cwiseProduct(c_e_ad.value());
652 [&](
const DenseVector& x) -> SparseMatrix {
654 return scaling.c_e.asDiagonal() * A_e.value();
656 [&](
const DenseVector& x) -> DenseVector {
658 return scaling.c_i.cwiseProduct(c_i_ad.value());
660 [&](
const DenseVector& x) -> SparseMatrix {
662 return scaling.c_i.asDiagonal() * A_i.value();
668 interior_point<Scalar>(matrix_callbacks, iteration_callbacks, options,
669#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
670 bound_constraint_mask,
675 if (options.diagnostics) {
676 slp::println(
"\nExit: {}", status);
680 VariableMatrix<Scalar>{m_decision_variables}.set_value(x);