diff --git a/examples/cpp/osqp_initializing_with_none.cpp b/examples/cpp/osqp_initializing_with_none.cpp new file mode 100644 index 000000000..fa761d485 --- /dev/null +++ b/examples/cpp/osqp_initializing_with_none.cpp @@ -0,0 +1,41 @@ +#include +#include "proxsuite/osqp/dense/dense.hpp" +#include // used for generating a random convex qp + +using namespace proxsuite::proxqp; +using T = double; +namespace pod = proxsuite::osqp::dense; + +int +main() +{ + dense::isize dim = 10; + dense::isize n_eq(0); + dense::isize n_in(0); + pod::QP qp(dim, n_eq, n_in); + T strong_convexity_factor(0.1); + T sparsity_factor(0.15); + // we generate a qp, so the function used from helpers.hpp is + // in proxqp namespace. The qp is in dense eigen format and + // you can control its sparsity ratio and strong convexity factor. + dense::Model qp_random = utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + qp.settings.default_mu_eq = T(1.E-2); + qp.settings.default_mu_in = T(1.E1); + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); // initialization with zero shape matrices + // it is equivalent to do qp.init(qp_random.H, qp_random.g, + // nullopt,nullopt,nullopt,nullopt,nullopt); + qp.solve(); + // print an optimal solution x,y and z + std::cout << "optimal x: " << qp.results.x << std::endl; + std::cout << "optimal y: " << qp.results.y << std::endl; + std::cout << "optimal z: " << qp.results.z << std::endl; +} \ No newline at end of file diff --git a/examples/cpp/osqp_initializing_with_none_without_api.cpp b/examples/cpp/osqp_initializing_with_none_without_api.cpp new file mode 100644 index 000000000..b40576dda --- /dev/null +++ b/examples/cpp/osqp_initializing_with_none_without_api.cpp @@ -0,0 +1,52 @@ +#include +#include "proxsuite/helpers/optional.hpp" +#include "proxsuite/osqp/dense/dense.hpp" +#include // used for generating a random convex qp + +using namespace proxsuite; +using T = double; + +namespace pp = proxsuite::proxqp; +namespace ppd = proxsuite::proxqp::dense; +namespace pod = proxsuite::osqp::dense; + +int +main() +{ + ppd::isize dim = 10; + ppd::isize n_eq(0); + ppd::isize n_in(0); + T strong_convexity_factor(0.1); + T sparsity_factor(0.15); + // we generate a qp, so the function used from helpers.hpp is + // in proxqp namespace. The qp is in dense eigen format and + // you can control its sparsity ratio and strong convexity factor. + ppd::Model qp_random = pp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + T eps_abs(1.E-5); + T eps_rel(0); + pp::Results results = pod::solve(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + nullopt, + nullopt, + nullopt, + eps_abs, + eps_rel, + nullopt, + T(1.E-2), + T(1.E-1)); + + // initialization with zero shape matrices + // it is equivalent to do dense::solve(qp_random.H, qp_random.g, + // nullopt,nullopt,nullopt,nullopt,nullopt); + // print an optimal solution x,y and z + std::cout << "optimal x: " << results.x << std::endl; + std::cout << "optimal y: " << results.y << std::endl; + std::cout << "optimal z: " << results.z << std::endl; +} \ No newline at end of file diff --git a/examples/cpp/osqp_overview-simple.cpp b/examples/cpp/osqp_overview-simple.cpp new file mode 100644 index 000000000..3d6c3d2ef --- /dev/null +++ b/examples/cpp/osqp_overview-simple.cpp @@ -0,0 +1,47 @@ +#include +#include +#include +#include // used for generating a random convex qp + +#include + +using namespace proxsuite::osqp; +using T = double; + +namespace pp = proxsuite::proxqp; +namespace ppd = proxsuite::proxqp::dense; +namespace pod = proxsuite::osqp::dense; + +int +main() +{ + T sparsity_factor = 0.15; + ppd::isize dim = 10; + ppd::isize n_eq(dim / 4); + ppd::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + + ppd::Model qp_random = pp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.default_mu_eq = T(1.E-2); + qp.settings.default_mu_in = T(1.E1); + + qp.settings.update_mu = true; + qp.settings.polish = true; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + std::cout << "optimal x: " << qp.results.x << std::endl; + std::cout << "optimal y: " << qp.results.y << std::endl; + std::cout << "optimal z: " << qp.results.z << std::endl; +} diff --git a/include/proxsuite/linalg/dense/ldlt.hpp b/include/proxsuite/linalg/dense/ldlt.hpp index b74bde51a..dec06338d 100644 --- a/include/proxsuite/linalg/dense/ldlt.hpp +++ b/include/proxsuite/linalg/dense/ldlt.hpp @@ -11,6 +11,9 @@ #include "proxsuite/linalg/dense/solve.hpp" #include +#include "proxsuite/proxqp/timings.hpp" +#include + namespace proxsuite { namespace linalg { namespace dense { @@ -718,29 +721,54 @@ struct Ldlt void factorize(Eigen::Ref mat /* NOLINT */, proxsuite::linalg::veg::dynstack::DynStackMut stack) { + // proxsuite::proxqp::Timer timer; + VEG_ASSERT(mat.rows() == mat.cols()); isize n = mat.rows(); + // timer.stop(); + // timer.start(); reserve_uninit(n); + // timer.stop(); + // std::cout << "Time of reserve_unit: " << timer.elapsed().user << + // std::endl; // Diff + // timer.start(); perm.resize_for_overwrite(n); perm_inv.resize_for_overwrite(n); maybe_sorted_diag.resize_for_overwrite(n); + // timer.stop(); + // std::cout << "Time of resize_for_overwrite: " << timer.elapsed().user << + // std::endl; // Diff + // timer.start(); proxsuite::linalg::dense::_detail::compute_permutation( // perm.ptr_mut(), perm_inv.ptr_mut(), util::diagonal(mat)); + // timer.stop(); + // std::cout << "Time of compute_permutation: " << timer.elapsed().user << + // std::endl; // Diff + // timer.start(); { LDLT_TEMP_MAT_UNINIT(T, work, n, n, stack); ld_col_mut() = mat; proxsuite::linalg::dense::_detail::apply_permutation_tri_lower( ld_col_mut(), work, perm.ptr()); } + // timer.stop(); + // std::cout << "Time of apply_permutation_tri_lower: " << + // timer.elapsed().user << std::endl; // Same timer.start(); for (isize i = 0; i < n; ++i) { maybe_sorted_diag[i] = ld_col()(i, i); } + // timer.stop(); + // std::cout << "Time of ld_col: " << timer.elapsed().user << std::endl; + // timer.start(); proxsuite::linalg::dense::factorize(ld_col_mut(), stack); + // timer.stop(); + // std::cout << "Time of proxsuite::lialg::dense::factorize: " << + // timer.elapsed().user << std::endl; // Diff } /*! diff --git a/include/proxsuite/osqp/dense/dense.hpp b/include/proxsuite/osqp/dense/dense.hpp new file mode 100644 index 000000000..5f8c5c798 --- /dev/null +++ b/include/proxsuite/osqp/dense/dense.hpp @@ -0,0 +1,13 @@ +// +// Copyright (c) 2025 INRIA +// +/** + * @file dense.hpp + */ + +#ifndef PROXSUITE_OSQP_DENSE_DENSE_HPP +#define PROXSUITE_OSQP_DENSE_DENSE_HPP + +#include "proxsuite/osqp/dense/wrapper.hpp" + +#endif /* end of include guard PROXSUITE_OSQP_DENSE_DENSE_HPP */ \ No newline at end of file diff --git a/include/proxsuite/osqp/dense/solver.hpp b/include/proxsuite/osqp/dense/solver.hpp new file mode 100644 index 000000000..e89cb412f --- /dev/null +++ b/include/proxsuite/osqp/dense/solver.hpp @@ -0,0 +1,1693 @@ +// +// Copyright (c) 2025 INRIA +// +/** + * @file solver.hpp + */ + +#ifndef PROXSUITE_OSQP_DENSE_SOLVER_HPP +#define PROXSUITE_OSQP_DENSE_SOLVER_HPP + +#include +#include +#include "proxsuite/proxqp/dense/solver.hpp" +#include "proxsuite/proxqp/dense/model.hpp" +#include "proxsuite/proxqp/dense/views.hpp" +#include "proxsuite/proxqp/dense/workspace.hpp" +#include "proxsuite/proxqp/dense/utils.hpp" +#include "proxsuite/proxqp/dense/fwd.hpp" +#include "proxsuite/proxqp/dense/preconditioner/ruiz.hpp" +#include "proxsuite/proxqp/settings.hpp" +#include "proxsuite/proxqp/results.hpp" +#include "proxsuite/proxqp/status.hpp" +#include "proxsuite/solvers/common/utils.hpp" +#include + +namespace proxsuite { +namespace osqp { +namespace dense { + +using namespace proxsuite::proxqp; +using namespace proxsuite::proxqp::dense; +namespace plv = proxsuite::linalg::veg; + +/*! + * Computes the scaled primal - dual residual ratio to update mu in OSQP. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpresults solver results. + */ +template +T +compute_update_ratio_primal_dual(const Model& qpmodel, + Results& qpresults, + Workspace& qpwork, + const bool box_constraints, + const HessianType hessian_type) +{ + proxsuite::common::global_primal_residual_scaled( + qpmodel, qpresults, qpwork, box_constraints); + T norm_primal_residual_scaled = infty_norm(qpwork.primal_residual_scaled); + + proxsuite::common::global_dual_residual_scaled( + qpresults, qpwork, qpmodel, box_constraints, hessian_type); + T norm_dual_residual_scaled = infty_norm(qpwork.dual_residual_scaled); + + T epsilon = 1e-10; + + T norm_Ax = infty_norm(qpwork.A_scaled * qpresults.x); + T norm_Cx = infty_norm(qpwork.C_scaled * qpresults.x); + if (box_constraints) { + norm_Cx = std::max( + norm_Cx, + infty_norm((qpwork.i_scaled.array() * qpresults.x.array()).matrix())); + } + T norm_zeta = + std::max(infty_norm(qpwork.zeta_eq), infty_norm(qpwork.zeta_in)); + T max_Ax_Cx = std::max(norm_Ax, norm_Cx); + T max_scale_primal = std::max(max_Ax_Cx, norm_zeta); + T primal_term = norm_primal_residual_scaled / (max_scale_primal + epsilon); + + T norm_Hx; + switch (hessian_type) { + case HessianType::Zero: + norm_Hx = 0; + break; + case HessianType::Dense: + norm_Hx = infty_norm( + qpwork.H_scaled.template selfadjointView() * qpresults.x); + break; + case HessianType::Diagonal: + norm_Hx = infty_norm( + (qpwork.H_scaled.diagonal().array() * qpresults.x.array()).matrix()); + break; + } + T norm_ATy = infty_norm(qpwork.A_scaled.transpose() * qpresults.y); + T norm_CTz = + infty_norm(qpwork.C_scaled.transpose() * qpresults.z.head(qpmodel.n_in)); + if (box_constraints) { + norm_CTz = std::max(norm_Cx, + infty_norm((qpwork.i_scaled.array() * + qpresults.z.tail(qpmodel.dim).array()) + .matrix())); + } + T norm_g = infty_norm(qpwork.g_scaled); + T max_Hx_g = std::max(norm_Hx, norm_g); + T max_ATy_CTz = std::max(norm_ATy, norm_CTz); + T max_scale_dual = std::max(max_Hx_g, max_ATy_CTz); + T dual_term = norm_dual_residual_scaled / (max_scale_dual + epsilon); + + T update_ratio_primal_dual = std::sqrt(primal_term / (dual_term + epsilon)); + return update_ratio_primal_dual; +} +/*! + * Updates the proximal parameters mu_eq and mu_in in the OSQP algorithm. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + * @param qpresults solver results. + */ +template +void +update_mu(const Settings& qpsettings, + const Model& qpmodel, + Results& qpresults, + Workspace& qpwork, + const bool box_constraints, + const isize n_constraints, + const DenseBackend dense_backend, + const HessianType hessian_type, + T& primal_feasibility_lhs, + T& primal_feasibility_lhs_new, + T& dual_feasibility_lhs, + T& dual_feasibility_lhs_new, + T& new_mu_eq, + T& new_mu_in, + T& new_mu_eq_inv, + T& new_mu_in_inv, + i64 iter) +{ + bool iteration_condition; + switch (qpsettings.update_mu_iteration_criteria) { + case UpdateMuIterationCriteria::FactorizationTime: { + if (iter == 0) { + qpwork.timer_between_updates.stop(); + qpwork.timer_between_updates.start(); + qpwork.time_since_last_update_mu = + qpwork.timer_between_updates.elapsed().user; + } else { + qpwork.time_since_last_update_mu = + qpwork.timer_between_updates.elapsed().user; + } + iteration_condition = qpwork.time_since_last_update_mu > + qpsettings.percentage_factorization_time_update_mu * + qpwork.factorization_time_complete_kkt; + break; + } + case UpdateMuIterationCriteria::FixedNumberIterations: { + iteration_condition = + iter - qpwork.last_iteration_update_mu > qpsettings.interval_update_mu; + } + } + + if (iteration_condition) { + T update_ratio_primal_dual = compute_update_ratio_primal_dual( + qpmodel, qpresults, qpwork, box_constraints, hessian_type); + + bool value_condition = + update_ratio_primal_dual > qpsettings.threshold_ratio_update_mu || + update_ratio_primal_dual < qpsettings.threshold_ratio_update_mu_inv; + + if (value_condition) { + new_mu_eq = qpresults.info.mu_eq / update_ratio_primal_dual; + new_mu_in = qpresults.info.mu_in / update_ratio_primal_dual; + new_mu_eq_inv = qpresults.info.mu_eq_inv * update_ratio_primal_dual; + new_mu_in_inv = qpresults.info.mu_in_inv * update_ratio_primal_dual; + + new_mu_eq = std::min(std::max(new_mu_eq, qpsettings.mu_min_eq), + qpsettings.mu_max_eq); + new_mu_in = std::min(std::max(new_mu_in, qpsettings.mu_min_in_osqp), + qpsettings.mu_max_in); + new_mu_eq_inv = + std::min(std::max(new_mu_eq_inv, qpsettings.mu_min_eq_inv), + qpsettings.mu_max_eq_inv); + new_mu_in_inv = + std::min(std::max(new_mu_in_inv, qpsettings.mu_min_in_inv), + qpsettings.mu_max_in_inv_osqp); + } + + if (primal_feasibility_lhs_new >= primal_feasibility_lhs && + dual_feasibility_lhs_new >= dual_feasibility_lhs && + (qpresults.info.mu_in <= T(1e-3) || qpresults.info.mu_in >= T(1e5))) { + new_mu_in = qpsettings.cold_reset_mu_in_osqp; + new_mu_eq = qpsettings.cold_reset_mu_eq_osqp; + new_mu_in_inv = qpsettings.cold_reset_mu_in_inv_osqp; + new_mu_eq_inv = qpsettings.cold_reset_mu_eq_inv_osqp; + } + + if (qpresults.info.mu_in != new_mu_in || + qpresults.info.mu_eq != new_mu_eq) { + { + ++qpresults.info.mu_updates; + } + mu_update(qpmodel, + qpresults, + qpwork, + n_constraints, + dense_backend, + new_mu_eq, + new_mu_in); + switch (qpsettings.update_mu_iteration_criteria) { + case UpdateMuIterationCriteria::FactorizationTime: { + qpwork.timer_between_updates.stop(); + qpwork.timer_between_updates.start(); + break; + } + case UpdateMuIterationCriteria::FixedNumberIterations: { + qpwork.last_iteration_update_mu = iter; + break; + } + } + qpresults.info.mu_eq = new_mu_eq; + qpresults.info.mu_in = new_mu_in; + qpresults.info.mu_eq_inv = new_mu_eq_inv; + qpresults.info.mu_in_inv = new_mu_in_inv; + } + } +} +/*! + * Checks the feasibility of the problem at the current step of the ADMM (OSQP) + * solver. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + * @param qpresults solver results. + * @param ruiz ruiz preconditioner. + */ +template +bool +is_infeasible(const Settings& qpsettings, + const Model& qpmodel, + Results& qpresults, + Workspace& qpwork, + const bool box_constraints, + preconditioner::RuizEquilibration& ruiz, + const HessianType hessian_type) +{ + // Get the intermediate data from the workspace + // Computation here as it was done implicitely in the corresponding part + // of the proxqp version in the function primal_dual_newton_semi_smooth + Vec dx = qpresults.x - qpwork.x_prev; + Vec dy = qpresults.y - qpwork.y_prev; + Vec dz = qpresults.z - qpwork.z_prev; + + auto& Hdx = qpwork.Hdx; + auto& Adx = qpwork.Adx; + auto& Cdx = qpwork.Cdx; + auto& ATdy = qpwork.CTz; + + switch (hessian_type) { + case HessianType::Zero: + break; + case HessianType::Dense: + Hdx.noalias() = + qpwork.H_scaled.template selfadjointView() * dx; + break; + case HessianType::Diagonal: +#ifndef NDEBUG + PROXSUITE_THROW_PRETTY(!qpwork.H_scaled.isDiagonal(), + std::invalid_argument, + "H is not diagonal."); +#endif + Hdx.array() = qpwork.H_scaled.diagonal().array() * dx.array(); + break; + } + Adx.noalias() = qpwork.A_scaled * dx; + ATdy.noalias() = qpwork.A_scaled.transpose() * dy; + + proxsuite::linalg::veg::dynstack::DynStackMut stack{ + proxsuite::linalg::veg::from_slice_mut, qpwork.ldl_stack.as_mut() + }; + LDLT_TEMP_VEC(T, CTdz, qpmodel.dim, stack); + if (qpmodel.n_in > 0) { + Cdx.head(qpmodel.n_in).noalias() = qpwork.C_scaled * dx; + CTdz.noalias() = qpwork.C_scaled.transpose() * dz.head(qpmodel.n_in); + } + if (box_constraints) { + qpwork.active_part_z.tail(qpmodel.dim) = dz.tail(qpmodel.dim); + qpwork.active_part_z.tail(qpmodel.dim).array() *= qpwork.i_scaled.array(); + CTdz.noalias() += qpwork.active_part_z.tail(qpmodel.dim); + + Cdx.tail(qpmodel.dim) = dx; + Cdx.tail(qpmodel.dim).array() *= qpwork.i_scaled.array(); + } + + // Call to intermadiate functions to check the feasibility + if (qpresults.info.iter_ext % qpsettings.frequence_infeasibility_check == 0 || + qpsettings.primal_infeasibility_solving) { + + bool is_primal_infeasible = + global_primal_residual_infeasibility(VectorViewMut{ from_eigen, ATdy }, + VectorViewMut{ from_eigen, CTdz }, + VectorViewMut{ from_eigen, dy }, + VectorViewMut{ from_eigen, dz }, + qpwork, + qpmodel, + qpsettings, + box_constraints, + ruiz); + + bool is_dual_infeasible = + global_dual_residual_infeasibility(VectorViewMut{ from_eigen, Adx }, + VectorViewMut{ from_eigen, Cdx }, + VectorViewMut{ from_eigen, Hdx }, + VectorViewMut{ from_eigen, dx }, + qpwork, + qpsettings, + qpmodel, + box_constraints, + ruiz); + + if (is_primal_infeasible) { + qpresults.info.status = QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE; + return true; + } else if (is_dual_infeasible) { + qpresults.info.status = QPSolverOutput::PROXQP_DUAL_INFEASIBLE; + return true; + } + } + return false; +} +/*! + * One iteration of the ADMM algorithm adapted in OSQP. + * + * Solves the linear system (KKT), then update the primal and dual variables. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + * @param qpresults solver results. + */ +template +void +admm_step(const Settings& qpsettings, + const Model& qpmodel, + Results& qpresults, + Workspace& qpwork, + const bool box_constraints, + const isize n_constraints, + const DenseBackend dense_backend) +{ + // Note: + // In the context of a library (proxsuite) to implement different solvers, we + // use the same inetrmediate functions (infeasibility, residuals, etc) and API + // than in the code of ProxQP Yet, the OSQP paper (see + // https://inria.hal.science/hal-03683733/file/Yet_another_QP_solver_for_robotics_and_beyond.pdf/) + // manages both the equality and inequality constraints in one matrix A. Thus, + // the following adapts the content of the paper to our library (eg matrices + // A, C, I for constraints). + + // Solve the linear system + Vec x_tilde; + Vec nu_eq; + Vec nu_in; + Vec zeta_tilde_eq; + Vec zeta_tilde_in; + + qpwork.rhs.setZero(); + qpwork.rhs.head(qpmodel.dim) = + qpresults.info.rho * qpresults.x - qpwork.g_scaled; + qpwork.rhs.segment(qpmodel.dim, qpmodel.n_eq) = + qpwork.b_scaled - qpresults.info.mu_eq * qpresults.y; + qpwork.rhs.tail(n_constraints) = + qpwork.zeta_in - qpresults.info.mu_in * qpresults.z; + + isize inner_pb_dim = qpmodel.dim + qpmodel.n_eq + n_constraints; + proxsuite::linalg::veg::dynstack::DynStackMut stack{ + proxsuite::linalg::veg::from_slice_mut, qpwork.ldl_stack.as_mut() + }; + solve_linear_system(qpwork.rhs, + qpmodel, + qpresults, + qpwork, + n_constraints, + dense_backend, + inner_pb_dim, + stack); + x_tilde = qpwork.rhs.head(qpmodel.dim); + nu_eq = qpwork.rhs.segment(qpmodel.dim, qpmodel.n_eq); + nu_in = qpwork.rhs.tail(n_constraints); + + // Update the variables + zeta_tilde_eq = + qpwork.b_scaled + qpresults.info.mu_eq * (nu_eq - qpresults.y); + zeta_tilde_in = qpwork.zeta_in + qpresults.info.mu_in * (nu_in - qpresults.z); + + qpresults.x = + qpsettings.alpha_osqp * x_tilde + (1 - qpsettings.alpha_osqp) * qpresults.x; + + qpwork.zeta_eq = qpwork.b_scaled; + Vec zeta_in_next = qpsettings.alpha_osqp * zeta_tilde_in + + (1 - qpsettings.alpha_osqp) * qpwork.zeta_in + + qpresults.info.mu_in * qpresults.z; + if (box_constraints) { + zeta_in_next.head(qpmodel.n_in) = qpwork.l_scaled.cwiseMax( + zeta_in_next.head(qpmodel.n_in).cwiseMin(qpwork.u_scaled)); + zeta_in_next.tail(qpmodel.dim) = qpwork.l_box_scaled.cwiseMax( + zeta_in_next.tail(qpmodel.dim).cwiseMin(qpwork.u_box_scaled)); + } else { + zeta_in_next = + qpwork.l_scaled.cwiseMax(zeta_in_next.cwiseMin(qpwork.u_scaled)); + } + + qpresults.y = qpresults.y + qpresults.info.mu_eq_inv * qpsettings.alpha_osqp * + (zeta_tilde_eq - qpwork.b_scaled); + qpresults.z = qpresults.z + + qpresults.info.mu_in_inv * + (qpsettings.alpha_osqp * zeta_tilde_in + + (1 - qpsettings.alpha_osqp) * qpwork.zeta_in - zeta_in_next); + + qpwork.zeta_in = zeta_in_next; +} +/*! + * Iterations of the ADMM algorithm in OSQP. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + * @param qpresults solver results. + * @param ruiz ruiz preconditioner. + * @param iter_start starting index of the first iteration. + */ +template +void +admm( // + const Settings& qpsettings, + const Model& qpmodel, + Results& qpresults, + Workspace& qpwork, + const bool box_constraints, + isize n_constraints, + const DenseBackend& dense_backend, + const HessianType& hessian_type, + preconditioner::RuizEquilibration& ruiz, + T& primal_feasibility_eq_rhs_0, + T& primal_feasibility_in_rhs_0, + T& primal_feasibility_eq_lhs, + T& primal_feasibility_in_lhs, + T& primal_feasibility_lhs, + T& dual_feasibility_lhs, + T& dual_feasibility_rhs_0, + T& dual_feasibility_rhs_1, + T& dual_feasibility_rhs_3, + T& rhs_duality_gap, + T& duality_gap, + T& scaled_eps, + T& scaled_eps_rel, + i64 iter_start) +{ + for (i64 iter = iter_start; iter < qpsettings.max_iter; ++iter) { + + T new_mu_in(qpresults.info.mu_in); + T new_mu_eq(qpresults.info.mu_eq); + T new_mu_in_inv(qpresults.info.mu_in_inv); + T new_mu_eq_inv(qpresults.info.mu_eq_inv); + + bool is_solved_qp = + proxsuite::common::is_solved(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + hessian_type, + ruiz, + common::QPSolver::OSQP, + primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0, + primal_feasibility_eq_lhs, + primal_feasibility_in_lhs, + primal_feasibility_lhs, + dual_feasibility_lhs, + dual_feasibility_rhs_0, + dual_feasibility_rhs_1, + dual_feasibility_rhs_3, + rhs_duality_gap, + duality_gap, + scaled_eps, + scaled_eps_rel, + iter); + if (is_solved_qp) { + if (iter == 0) { + qpresults.info.admm_solved_at_init = true; + } + break; + } + + qpresults.info.iter_ext += 1; + + qpwork.x_prev = qpresults.x; + qpwork.y_prev = qpresults.y; + qpwork.z_prev = qpresults.z; + + proxsuite::common::compute_scaled_primal_residual_ineq( + qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + ruiz, + common::QPSolver::OSQP); + + admm_step(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + n_constraints, + dense_backend); + + bool is_infeasible_qp = is_infeasible(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + ruiz, + hessian_type); + if (is_infeasible_qp) { + break; + } + + qpwork.active_set_up.array() = + (qpwork.primal_residual_in_scaled_up.array() > + 0); // {zeta_in - u + z > 0} + qpwork.active_set_low.array() = + (qpresults.si.array() < 0); // {zeta_in - l + z < 0} + + T primal_feasibility_lhs_new(primal_feasibility_lhs); + T dual_feasibility_lhs_new(dual_feasibility_lhs); + proxsuite::common::update_solver_status(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + hessian_type, + ruiz, + primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0, + primal_feasibility_eq_lhs, + primal_feasibility_in_lhs, + primal_feasibility_lhs_new, + dual_feasibility_lhs_new, + dual_feasibility_rhs_0, + dual_feasibility_rhs_1, + dual_feasibility_rhs_3, + rhs_duality_gap, + duality_gap, + scaled_eps, + scaled_eps_rel); + + if (qpsettings.update_mu) { + update_mu(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + n_constraints, + dense_backend, + hessian_type, + primal_feasibility_lhs, + primal_feasibility_lhs_new, + dual_feasibility_lhs, + dual_feasibility_lhs_new, + new_mu_eq, + new_mu_in, + new_mu_eq_inv, + new_mu_in_inv, + iter); + } + + } // outer iterations loop +} +/*! + * Find the active sets in solution polishing. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + * @param qpresults solver results. + */ +template +void +find_active_sets(const Settings& qpsettings, + const Model& qpmodel, + Results& qpresults, + Workspace& qpwork, + const bool box_constraints, + preconditioner::RuizEquilibration& ruiz, + VecBool& active_constraints_eq, + isize& num_active_constraints_eq, + isize& num_active_constraints_eq_low, + isize& num_active_constraints_eq_up, + VecBool& active_constraints_ineq, + isize& num_active_constraints_ineq, + isize& num_active_constraints_ineq_low, + isize& num_active_constraints_ineq_up, + isize& num_active_constraints, + isize& inner_pb_dim) +{ + // Equality + qpwork.active_set_low_eq.array() = (qpresults.y.array() < 0); + qpwork.active_set_up_eq.array() = (qpresults.y.array() > 0); + active_constraints_eq = qpwork.active_set_up_eq || qpwork.active_set_low_eq; + num_active_constraints_eq = active_constraints_eq.count(); + num_active_constraints_eq_low = qpwork.active_set_low_eq.count(); + num_active_constraints_eq_up = qpwork.active_set_up_eq.count(); + + // Inequality + if (qpresults.info.admm_solved_at_init) { + // Compute active sets that could not be computed in ADMM + qpwork.z_prev = qpresults.z; + if (box_constraints) { + qpwork.zeta_in.head(qpmodel.n_in) = qpwork.C_scaled * qpresults.x; + qpwork.zeta_in.tail(qpmodel.dim).array() = + qpwork.i_scaled.array() * qpresults.x.array(); + } else { + qpwork.zeta_in = qpwork.C_scaled * qpresults.x; + } + + proxsuite::common::compute_scaled_primal_residual_ineq( + qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + ruiz, + common::QPSolver::OSQP); + + qpwork.active_set_up.array() = + (qpwork.primal_residual_in_scaled_up.array() > + 0); // {zeta_in - u + z > 0} + qpwork.active_set_low.array() = + (qpresults.si.array() < 0); // {zeta_in - l + z < 0} + } + active_constraints_ineq = qpwork.active_set_up || qpwork.active_set_low; + num_active_constraints_ineq = active_constraints_ineq.count(); + num_active_constraints_ineq_low = qpwork.active_set_low.count(); + num_active_constraints_ineq_up = qpwork.active_set_up.count(); + + num_active_constraints = + num_active_constraints_eq + num_active_constraints_ineq; + + inner_pb_dim = qpmodel.dim + num_active_constraints; +} +/*! + * Build the reduced matrices of constraints in polishing. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + * @param qpresults solver results. + */ +template +void +build_reduced_constraints_matrices( // + const Settings& qpsettings, + const Model& qpmodel, + Results& qpresults, + Workspace& qpwork, + const isize n_constraints, + Mat& A_low, + Mat& A_up, + Mat& C_low, + Mat& C_up) +{ + isize low_index = 0; + isize up_index = 0; + for (isize i = 0; i < qpmodel.n_eq; ++i) { + if (qpwork.active_set_low_eq(i)) { + A_low.row(low_index) = qpwork.A_scaled.row(i); + ++low_index; + } + if (qpwork.active_set_up_eq(i)) { + A_up.row(up_index) = qpwork.A_scaled.row(i); + ++up_index; + } + } + + low_index = 0; + up_index = 0; + Vec tmp_low(qpmodel.dim); + Vec tmp_up(qpmodel.dim); + tmp_low.setZero(); + tmp_up.setZero(); + for (isize i = 0; i < n_constraints; ++i) { + if (qpwork.active_set_low(i)) { + if (i < qpmodel.n_in) { + C_low.row(low_index) = qpwork.C_scaled.row(i); + } else { + tmp_low(i - qpmodel.n_in) = qpwork.i_scaled(i - qpmodel.n_in); + C_low.row(low_index) = tmp_low; + tmp_low(i - qpmodel.n_in) = 0.; + } + ++low_index; + } + if (qpwork.active_set_up(i)) { + if (i < qpmodel.n_in) { + C_up.row(up_index) = qpwork.C_scaled.row(i); + } else { + tmp_up(i - qpmodel.n_in) = qpwork.i_scaled(i - qpmodel.n_in); + C_up.row(up_index) = tmp_up; + tmp_up(i - qpmodel.n_in) = 0.; + } + ++up_index; + } + } +} +/*! + * Build the matrices K and K + Delta_K in polishing. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + */ +template +void +build_kkt_matrices_polishing( // + const Settings& qpsettings, + const Model& qpmodel, + Workspace& qpwork, + const HessianType hessian_type, + Mat& k_polish, + Mat& k_plus_delta_k_polish, + Mat A_low, + Mat A_up, + Mat C_low, + Mat C_up, + isize num_active_constraints_eq_low, + isize num_active_constraints_ineq_low, + isize num_active_constraints_eq_up, + isize num_active_constraints_ineq_up, + isize num_active_constraints) +{ + // Construction of K + isize row; + isize col; + + switch (hessian_type) { + case HessianType::Dense: + k_polish.topLeftCorner(qpmodel.dim, qpmodel.dim) = qpwork.H_scaled; + break; + case HessianType::Zero: + k_polish.topLeftCorner(qpmodel.dim, qpmodel.dim).setZero(); + break; + case HessianType::Diagonal: + k_polish.topLeftCorner(qpmodel.dim, qpmodel.dim) = qpwork.H_scaled; + break; + } + + col = qpmodel.dim; + k_polish.block(0, col, qpmodel.dim, num_active_constraints_eq_low) = + A_low.transpose(); + + col += num_active_constraints_eq_low; + k_polish.block(0, col, qpmodel.dim, num_active_constraints_ineq_low) = + C_low.transpose(); + + col += num_active_constraints_ineq_low; + k_polish.block(0, col, qpmodel.dim, num_active_constraints_eq_up) = + A_up.transpose(); + + col += num_active_constraints_eq_up; + k_polish.block(0, col, qpmodel.dim, num_active_constraints_ineq_up) = + C_up.transpose(); + + row = qpmodel.dim; + k_polish.block(row, 0, num_active_constraints_eq_low, qpmodel.dim) = A_low; + + row += num_active_constraints_eq_low; + k_polish.block(row, 0, num_active_constraints_ineq_low, qpmodel.dim) = C_low; + + row += num_active_constraints_ineq_low; + k_polish.block(row, 0, num_active_constraints_eq_up, qpmodel.dim) = A_up; + + row += num_active_constraints_eq_up; + k_polish.block(row, 0, num_active_constraints_ineq_up, qpmodel.dim) = C_up; + + k_polish.bottomRightCorner(num_active_constraints, num_active_constraints) + .setZero(); + + // Construction of K + Delta_K + k_plus_delta_k_polish = k_polish; + k_plus_delta_k_polish.topLeftCorner(qpmodel.dim, qpmodel.dim) + .diagonal() + .array() += qpsettings.delta; + k_plus_delta_k_polish + .bottomRightCorner(num_active_constraints, num_active_constraints) + .diagonal() + .array() -= qpsettings.delta; +} +/*! + * Build the right hand side (-g, l_L, u_U) in polishing. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + */ +template +void +build_rhs_polishing( // + const Settings& qpsettings, + const Model& qpmodel, + Workspace& qpwork, + const HessianType hessian_type, + const isize n_constraints, + Vec& rhs_polish, + isize num_active_constraints_eq_low, + isize num_active_constraints_ineq_low, + isize num_active_constraints_eq_up, + isize num_active_constraints_ineq_up) +{ + isize low_index = 0; + isize up_index = 0; + Vec b_low(num_active_constraints_eq_low); + Vec b_up(num_active_constraints_eq_up); + for (isize i = 0; i < qpmodel.n_eq; ++i) { + if (qpwork.active_set_low_eq(i)) { + b_low(low_index) = qpwork.b_scaled(i); + ++low_index; + } + if (qpwork.active_set_up_eq(i)) { + b_up(up_index) = qpwork.b_scaled(i); + ++up_index; + } + } + + low_index = 0; + up_index = 0; + Vec l_low(num_active_constraints_ineq_low); + Vec u_up(num_active_constraints_ineq_up); + for (isize i = 0; i < n_constraints; ++i) { + if (qpwork.active_set_low(i)) { + if (i < qpmodel.n_in) { + l_low(low_index) = qpwork.l_scaled(i); + } else { + l_low(low_index) = qpwork.l_box_scaled(i - qpmodel.n_in); + } + ++low_index; + } + if (qpwork.active_set_up(i)) { + if (i < qpmodel.n_in) { + u_up(up_index) = qpwork.u_scaled(i); + } else { + u_up(up_index) = qpwork.u_box_scaled(i - qpmodel.n_in); + } + ++up_index; + } + } + + isize row; + isize col; + + row = qpmodel.dim; + rhs_polish.head(row) = -qpwork.g_scaled; + rhs_polish.segment(row, num_active_constraints_eq_low) = b_low; + + row += num_active_constraints_eq_low; + rhs_polish.segment(row, num_active_constraints_ineq_low) = l_low; + + row += num_active_constraints_ineq_low; + rhs_polish.segment(row, num_active_constraints_eq_up) = b_up; + rhs_polish.tail(num_active_constraints_ineq_up) = u_up; +} +/*! + * Iterative refinement in polishing. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + */ +template +void +iterative_refinement_polishing( // + const Settings& qpsettings, + const Model& qpmodel, + Results& qpresults, + Workspace& qpwork, + const DenseBackend& dense_backend, + const isize n_constraints, + Vec& hat_t, + Vec rhs_polish, + Mat k_polish, + isize inner_pb_dim, + plv::dynstack::DynStackMut& stack) +{ + // Solve the reduced system before iterative refinement + hat_t = rhs_polish; + solve_linear_system(hat_t, + qpmodel, + qpresults, + qpwork, + n_constraints, + dense_backend, + inner_pb_dim, + stack); + + // Iterative refinement + Vec rhs_polish_refine(inner_pb_dim); + Vec delta_hat_t(inner_pb_dim); + + for (i64 iter = 0; iter < qpsettings.polish_refine_iter; ++iter) { + rhs_polish_refine = rhs_polish - k_polish * hat_t; + delta_hat_t = rhs_polish_refine; + + solve_linear_system(delta_hat_t, + qpmodel, + qpresults, + qpwork, + n_constraints, + dense_backend, + inner_pb_dim, + stack); + + hat_t = hat_t + delta_hat_t; + } +} +/*! + * Update primal and dual variables in polishing. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpresults solver results. + */ +template +void +update_variables_polishing( // + const Model& qpmodel, + Results& qpresults, + Workspace& qpwork, + const bool box_constraints, + const isize n_constraints, + Vec hat_t, + isize num_active_constraints_eq_low, + isize num_active_constraints_eq_up, + isize num_active_constraints_ineq_low) +{ + // Get (x, y, z) from hat_t + qpresults.x = hat_t.head(qpmodel.dim); + + isize low_index = 0; + isize up_index = 0; + for (isize i = 0; i < qpmodel.n_eq; ++i) { + if (qpwork.active_set_low_eq(i)) { + qpresults.y(i) = hat_t(qpmodel.dim + low_index); + ++low_index; + } + if (qpwork.active_set_up_eq(i)) { + qpresults.y(i) = hat_t(qpmodel.dim + num_active_constraints_eq_low + + num_active_constraints_ineq_low + up_index); + ++up_index; + } + } + + low_index = 0; + up_index = 0; + for (isize i = 0; i < n_constraints; ++i) { + if (qpwork.active_set_low(i)) { + qpresults.z(i) = + hat_t(qpmodel.dim + num_active_constraints_eq_low + low_index); + ++low_index; + } + if (qpwork.active_set_up(i)) { + qpresults.z(i) = hat_t(qpmodel.dim + num_active_constraints_eq_low + + num_active_constraints_ineq_low + + num_active_constraints_eq_up + up_index); + ++up_index; + } + } + + // Projection of the dual solution into the normal cone N_C(zeta) + Vec tmp_z(n_constraints); + tmp_z = qpresults.z + qpwork.zeta_in; + if (box_constraints) { + qpwork.zeta_in.head(qpmodel.n_in) = qpwork.l_scaled.cwiseMax( + qpresults.z.head(qpmodel.n_in).cwiseMin(qpwork.u_scaled)); + qpwork.zeta_in.tail(qpmodel.dim) = qpwork.l_box_scaled.cwiseMax( + qpresults.z.tail(qpmodel.dim).cwiseMin(qpwork.u_box_scaled)); + } else { + qpresults.z = + qpwork.l_scaled.cwiseMax(qpwork.zeta_in.cwiseMin(qpwork.u_scaled)); + } + qpresults.z = tmp_z - qpwork.zeta_in; +} +/*! + * Update residuals in polishing. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpresults solver results. + * @param qpsettings solver settings. + */ +template +void +update_residuals_polishing( // + const Settings& qpsettings, + const Model& qpmodel, + Results& qpresults, + Workspace& qpwork, + const bool box_constraints, + const isize n_constraints, + const HessianType hessian_type, + preconditioner::RuizEquilibration& ruiz, + T& primal_feasibility_lhs, + T& primal_feasibility_eq_rhs_0, + T& primal_feasibility_in_rhs_0, + T& primal_feasibility_eq_lhs, + T& primal_feasibility_in_lhs, + T& dual_feasibility_lhs, + T& dual_feasibility_rhs_0, + T& dual_feasibility_rhs_1, + T& dual_feasibility_rhs_3, + T& rhs_duality_gap, + T& duality_gap, + bool& is_feasible) +{ + global_primal_residual(qpmodel, + qpresults, + qpsettings, + qpwork, + ruiz, + box_constraints, + primal_feasibility_lhs, + primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0, + primal_feasibility_eq_lhs, + primal_feasibility_in_lhs); + + bool is_primal_feasible = + primal_feasibility_lhs <= + (qpsettings.eps_abs + + qpsettings.eps_rel * + std::max(primal_feasibility_eq_rhs_0, primal_feasibility_in_rhs_0)); + qpresults.info.pri_res = primal_feasibility_lhs; + + if (is_primal_feasible) { + global_dual_residual(qpresults, + qpwork, + qpmodel, + box_constraints, + ruiz, + dual_feasibility_lhs, + dual_feasibility_rhs_0, + dual_feasibility_rhs_1, + dual_feasibility_rhs_3, + rhs_duality_gap, + duality_gap, + hessian_type); + qpresults.info.dua_res = dual_feasibility_lhs; + qpresults.info.duality_gap = duality_gap; + + bool is_dual_feasible = + dual_feasibility_lhs <= + (qpsettings.eps_abs + + qpsettings.eps_rel * + std::max( + std::max(dual_feasibility_rhs_3, dual_feasibility_rhs_0), + std::max(dual_feasibility_rhs_1, qpwork.dual_feasibility_rhs_2))); + + if (is_dual_feasible) { + if (qpsettings.check_duality_gap) { + if (std::fabs(qpresults.info.duality_gap) <= + qpsettings.eps_duality_gap_abs + + qpsettings.eps_duality_gap_rel * rhs_duality_gap) { + is_feasible = true; + } + } else { + is_feasible = true; + } + } + } +} +/*! + * Check the success of solution polishing. + * + * @param qpresults solver results. + * @param qpsettings solver settings. + */ +template +void +check_success_polishing( // + const Settings& qpsettings, + Results& qpresults, + T pri_res_admm, + T dua_res_admm, + T duality_gap_admm, + bool is_feasible, + bool& polish_success) +{ + if (qpsettings.check_duality_gap) { + bool polish_success_primal_dual = + ((qpresults.info.pri_res < pri_res_admm) && + (qpresults.info.dua_res < dua_res_admm)) + + || ((qpresults.info.pri_res < pri_res_admm) && + (qpresults.info.dua_res < 1e-10)) + + || ((qpresults.info.dua_res < dua_res_admm) && + (qpresults.info.pri_res < 1e-10)); + + polish_success = + ((qpresults.info.duality_gap < duality_gap_admm) && + polish_success_primal_dual) + + || ((qpresults.info.duality_gap < 1e-9) && polish_success_primal_dual); + } else { + polish_success = ((qpresults.info.pri_res < pri_res_admm) && + (qpresults.info.dua_res < dua_res_admm)) + + || ((qpresults.info.pri_res < pri_res_admm) && + (qpresults.info.dua_res < 1e-10)) + + || ((qpresults.info.dua_res < dua_res_admm) && + (qpresults.info.pri_res < 1e-10)); + } + + if (!is_feasible) { + polish_success = false; + } +} +/*! + * Print polishing line after the ADMM iterations. + * + * @param qpresults solver results. + * @param qpsettings solver settings. + */ +template +void +print_polishing_line( // + const Settings& qpsettings, + Results& qpresults, + bool is_feasible) +{ + std::cout << "\033[1;34m[polishing]\033[0m" << std::endl; + std::cout << std::scientific << std::setw(2) << std::setprecision(2) + << " | primal residual=" << qpresults.info.pri_res + << " | dual residual=" << qpresults.info.dua_res + << " | duality gap=" << qpresults.info.duality_gap + << " | delta=" << qpsettings.delta << std::endl; + switch (qpresults.info.polish_status) { + case PolishStatus::POLISH_SUCCEED: { + std::cout << "\033[1;34m[polishing: succeed]\033[0m" << std::endl; + break; + } + case PolishStatus::POLISH_FAILED: { + if (!is_feasible) { + std::cout << "\033[1;34m[polishing: infeasible solution. " + << (qpsettings.resume_admm ? "Resume ADMM" : "") << "]\033[0m" + << std::endl; + } else { + std::cout << "\033[1;34m[polishing: failed. " + << (qpsettings.resume_admm ? "Resume ADMM" : "") << "]\033[0m" + << std::endl; + } + break; + } + case PolishStatus::POLISH_NO_ACTIVE_SET_FOUND: { + break; + } + case PolishStatus::POLISH_NOT_RUN: { + break; + } + } +} +/*! + * Solution polishing. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + * @param qpresults solver results. + */ +template +void +polish(const Settings& qpsettings, + const Model& qpmodel, + Results& qpresults, + Workspace& qpwork, + const bool box_constraints, + const isize n_constraints, + const DenseBackend dense_backend, + const HessianType hessian_type, + preconditioner::RuizEquilibration& ruiz, + T& primal_feasibility_lhs, + T& primal_feasibility_eq_rhs_0, + T& primal_feasibility_in_rhs_0, + T& primal_feasibility_eq_lhs, + T& primal_feasibility_in_lhs, + T& dual_feasibility_lhs, + T& dual_feasibility_rhs_0, + T& dual_feasibility_rhs_1, + T& dual_feasibility_rhs_3, + T& rhs_duality_gap, + T& duality_gap) +{ + + // Update number of polish calls + qpresults.info.polish_calls += 1; + + // Timing polishing + qpwork.timer_polish.stop(); + qpwork.timer_polish.start(); + + // ADMM solution + auto x_admm = qpresults.x; + auto y_admm = qpresults.y; + auto z_admm = qpresults.z; + auto zeta_in_admm = qpwork.zeta_in; + + auto pri_res_admm = qpresults.info.pri_res; + auto dua_res_admm = qpresults.info.dua_res; + auto duality_gap_admm = qpresults.info.duality_gap; + + // Upper and lower active constraints + VecBool active_constraints_eq; + isize num_active_constraints_eq; + isize num_active_constraints_eq_low; + isize num_active_constraints_eq_up; + + VecBool active_constraints_ineq; + isize num_active_constraints_ineq; + isize num_active_constraints_ineq_low; + isize num_active_constraints_ineq_up; + + isize num_active_constraints; + isize inner_pb_dim; + + find_active_sets(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + ruiz, + active_constraints_eq, + num_active_constraints_eq, + num_active_constraints_eq_low, + num_active_constraints_eq_up, + active_constraints_ineq, + num_active_constraints_ineq, + num_active_constraints_ineq_low, + num_active_constraints_ineq_up, + num_active_constraints, + inner_pb_dim); + + if (num_active_constraints == 0) { + qpresults.info.polish_status = PolishStatus::POLISH_NO_ACTIVE_SET_FOUND; + if (qpsettings.verbose) { + std::cout << "\033[1;34m[polishing: no active set found. " + << (qpsettings.resume_admm ? "Resume ADMM" : "") << "]\033[0m" + << std::endl; + } + return; + } + + // Build the reduced matrices of the constraints + Mat A_low(num_active_constraints_eq_low, qpmodel.dim); + Mat A_up(num_active_constraints_eq_up, qpmodel.dim); + + Mat C_low(num_active_constraints_ineq_low, qpmodel.dim); + Mat C_up(num_active_constraints_ineq_up, qpmodel.dim); + + build_reduced_constraints_matrices(qpsettings, + qpmodel, + qpresults, + qpwork, + n_constraints, + A_low, + A_up, + C_low, + C_up); + + // Build the KKT and regularized KKT matrices + Mat k_polish(inner_pb_dim, inner_pb_dim); + Mat k_plus_delta_k_polish(inner_pb_dim, inner_pb_dim); + + build_kkt_matrices_polishing(qpsettings, + qpmodel, + qpwork, + hessian_type, + k_polish, + k_plus_delta_k_polish, + A_low, + A_up, + C_low, + C_up, + num_active_constraints_eq_low, + num_active_constraints_ineq_low, + num_active_constraints_eq_up, + num_active_constraints_ineq_up, + num_active_constraints); + + proxsuite::linalg::veg::dynstack::DynStackMut stack{ + proxsuite::linalg::veg::from_slice_mut, + qpwork.ldl_stack.as_mut(), + }; + + qpwork.ldl.factorize(k_plus_delta_k_polish.transpose(), stack); + + // Construction of rhs_polish + Vec rhs_polish(inner_pb_dim); + + build_rhs_polishing(qpsettings, + qpmodel, + qpwork, + hessian_type, + n_constraints, + rhs_polish, + num_active_constraints_eq_low, + num_active_constraints_ineq_low, + num_active_constraints_eq_up, + num_active_constraints_ineq_up); + + // Iterative refinement + Vec hat_t(inner_pb_dim); + + iterative_refinement_polishing(qpsettings, + qpmodel, + qpresults, + qpwork, + dense_backend, + n_constraints, + hat_t, + rhs_polish, + k_polish, + inner_pb_dim, + stack); + + // Update of (x, y, z) + update_variables_polishing(qpmodel, + qpresults, + qpwork, + box_constraints, + n_constraints, + hat_t, + num_active_constraints_eq_low, + num_active_constraints_eq_up, + num_active_constraints_ineq_low); + + // Timing polishing + qpwork.time_polishing = qpwork.timer_polish.elapsed().user; + + // Update of residuals + bool is_feasible = false; + + update_residuals_polishing(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + n_constraints, + hessian_type, + ruiz, + primal_feasibility_lhs, + primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0, + primal_feasibility_eq_lhs, + primal_feasibility_in_lhs, + dual_feasibility_lhs, + dual_feasibility_rhs_0, + dual_feasibility_rhs_1, + dual_feasibility_rhs_3, + rhs_duality_gap, + duality_gap, + is_feasible); + + // Check polish success + bool polish_success; + + check_success_polishing(qpsettings, + qpresults, + pri_res_admm, + dua_res_admm, + duality_gap_admm, + is_feasible, + polish_success); + + if (polish_success) { + qpresults.info.polish_status = PolishStatus::POLISH_SUCCEED; + } else { + qpresults.info.polish_status = PolishStatus::POLISH_FAILED; + } + + // Print polishing line + if (qpsettings.verbose) { + print_polishing_line(qpsettings, qpresults, is_feasible); + } + + // Go back if polish failed + if (!polish_success) { + qpresults.x = x_admm; + qpresults.y = y_admm; + qpresults.z = z_admm; + qpwork.zeta_in = zeta_in_admm; + + qpresults.info.pri_res = pri_res_admm; + qpresults.info.dua_res = dua_res_admm; + qpresults.info.duality_gap = duality_gap_admm; + } +} +/*! + * Executes the OSQP algorithm. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + * @param qpresults solver results. + * @param ruiz ruiz preconditioner. + */ +template +void +qp_solve( // + const Settings& qpsettings, + const Model& qpmodel, + Results& qpresults, + Workspace& qpwork, + const bool box_constraints, + const DenseBackend& dense_backend, + const HessianType& hessian_type, + preconditioner::RuizEquilibration& ruiz) +{ + PROXSUITE_EIGEN_MALLOC_NOT_ALLOWED(); + + // Setup + proxsuite::common::setup_solver(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + dense_backend, + hessian_type, + ruiz, + common::QPSolver::OSQP); + + isize n_constraints(qpmodel.n_in); + if (box_constraints) { + n_constraints += qpmodel.dim; + } + + T primal_feasibility_eq_rhs_0(0); + T primal_feasibility_in_rhs_0(0); + T dual_feasibility_rhs_0(0); + T dual_feasibility_rhs_1(0); + T dual_feasibility_rhs_3(0); + T primal_feasibility_lhs(0); + T primal_feasibility_eq_lhs(0); + T primal_feasibility_in_lhs(0); + T dual_feasibility_lhs(0); + + T duality_gap(0); + T rhs_duality_gap(0); + + bool polish_solution = qpsettings.polish; + if (qpmodel.n_in + qpmodel.n_eq == 0) { + polish_solution = false; + } + + T scaled_eps; + T scaled_eps_rel; + if (polish_solution) { + if (qpsettings.high_accuracy) { + scaled_eps = 1e-5; + scaled_eps_rel = (qpsettings.eps_rel == 0) ? 0 : 1e-5; + } else { + scaled_eps = 1e-3; + scaled_eps_rel = (qpsettings.eps_rel == 0) ? 0 : 1e-3; + } + } else { + scaled_eps = qpsettings.eps_abs; + scaled_eps_rel = qpsettings.eps_rel; + } + + // Body of solve + admm(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + n_constraints, + dense_backend, + hessian_type, + ruiz, + primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0, + primal_feasibility_eq_lhs, + primal_feasibility_in_lhs, + primal_feasibility_lhs, + dual_feasibility_lhs, + dual_feasibility_rhs_0, + dual_feasibility_rhs_1, + dual_feasibility_rhs_3, + rhs_duality_gap, + duality_gap, + scaled_eps, + scaled_eps_rel, + 0); + + if (polish_solution) { + if (qpresults.info.status == QPSolverOutput::PROXQP_SOLVED) { + polish(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + n_constraints, + dense_backend, + hessian_type, + ruiz, + primal_feasibility_lhs, + primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0, + primal_feasibility_eq_lhs, + primal_feasibility_in_lhs, + dual_feasibility_lhs, + dual_feasibility_rhs_0, + dual_feasibility_rhs_1, + dual_feasibility_rhs_3, + rhs_duality_gap, + duality_gap); + + bool resume = + qpsettings.resume_admm && + (qpresults.info.polish_status == + PolishStatus::POLISH_NO_ACTIVE_SET_FOUND || + qpresults.info.polish_status == PolishStatus::POLISH_FAILED); + + if (resume) { + qpresults.info.resumed_admm = true; + + if (qpresults.info.polish_status == PolishStatus::POLISH_FAILED) { + // Retrieve the complete KKT matrix of the problem as the failed + // polish changed the stack and qpwork.ldl + proxsuite::proxqp::dense::setup_factorization( + qpwork, qpmodel, qpresults, dense_backend, hessian_type); + proxsuite::common::setup_factorization_complete_kkt( + qpwork, qpmodel, qpresults, dense_backend, n_constraints); + } + + if (qpsettings.high_accuracy == false && qpsettings.try_high_accuracy) { + // Resume with ADMM at "high" precision (1e-5), then polish + // prefered when ADMM only at settings precision (typically 1e-9) + // may not converge + qpresults.info.tried_high_accuracy = true; + + scaled_eps = 1e-5; + scaled_eps_rel = (qpsettings.eps_rel == 0) ? 0 : 1e-5; + admm(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + n_constraints, + dense_backend, + hessian_type, + ruiz, + primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0, + primal_feasibility_eq_lhs, + primal_feasibility_in_lhs, + primal_feasibility_lhs, + dual_feasibility_lhs, + dual_feasibility_rhs_0, + dual_feasibility_rhs_1, + dual_feasibility_rhs_3, + rhs_duality_gap, + duality_gap, + scaled_eps, + scaled_eps_rel, + qpresults.info.iter_ext); + + if (qpresults.info.status == QPSolverOutput::PROXQP_SOLVED) { + polish(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + n_constraints, + dense_backend, + hessian_type, + ruiz, + primal_feasibility_lhs, + primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0, + primal_feasibility_eq_lhs, + primal_feasibility_in_lhs, + dual_feasibility_lhs, + dual_feasibility_rhs_0, + dual_feasibility_rhs_1, + dual_feasibility_rhs_3, + rhs_duality_gap, + duality_gap); + + bool resume_second = + (qpresults.info.polish_status == + PolishStatus::POLISH_NO_ACTIVE_SET_FOUND || + qpresults.info.polish_status == PolishStatus::POLISH_FAILED); + + if (resume_second) { + // Resume with ADMM at OSQP settings precision + scaled_eps = qpsettings.eps_abs; + scaled_eps_rel = qpsettings.eps_rel; + admm(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + n_constraints, + dense_backend, + hessian_type, + ruiz, + primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0, + primal_feasibility_eq_lhs, + primal_feasibility_in_lhs, + primal_feasibility_lhs, + dual_feasibility_lhs, + dual_feasibility_rhs_0, + dual_feasibility_rhs_1, + dual_feasibility_rhs_3, + rhs_duality_gap, + duality_gap, + scaled_eps, + scaled_eps_rel, + qpresults.info.iter_ext); + } + } + } else { // Resume with ADMM at OSQP settings precision + scaled_eps = qpsettings.eps_abs; + scaled_eps_rel = qpsettings.eps_rel; + admm(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + n_constraints, + dense_backend, + hessian_type, + ruiz, + primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0, + primal_feasibility_eq_lhs, + primal_feasibility_in_lhs, + primal_feasibility_lhs, + dual_feasibility_lhs, + dual_feasibility_rhs_0, + dual_feasibility_rhs_1, + dual_feasibility_rhs_3, + rhs_duality_gap, + duality_gap, + scaled_eps, + scaled_eps_rel, + qpresults.info.iter_ext); + } + } + } + } + + // End of solve + proxsuite::common::unscale_solver( + qpsettings, qpmodel, qpresults, box_constraints, ruiz); + proxsuite::common::compute_objective(qpmodel, qpresults); + if (qpsettings.compute_timings) { + proxsuite::common::compute_timings(qpresults, qpwork); + } + + if (qpsettings.verbose) { + proxsuite::common::print_solver_statistics( + qpsettings, qpresults, common::QPSolver::OSQP); + } + + proxsuite::common::prepare_next_solve(qpresults, qpwork); + + PROXSUITE_EIGEN_MALLOC_ALLOWED(); +} + +} // namespace dense +} // namespace osqp +} // namespace proxsuite + +#endif /* end of include guard PROXSUITE_OSQP_DENSE_SOLVER_HPP */ \ No newline at end of file diff --git a/include/proxsuite/osqp/dense/wrapper.hpp b/include/proxsuite/osqp/dense/wrapper.hpp new file mode 100644 index 000000000..59f61bdc6 --- /dev/null +++ b/include/proxsuite/osqp/dense/wrapper.hpp @@ -0,0 +1,314 @@ +// +// Copyright (c) 2025 INRIA +// +/** + * @file wrapper.hpp + */ + +#ifndef PROXSUITE_OSQP_DENSE_WRAPPER_HPP +#define PROXSUITE_OSQP_DENSE_WRAPPER_HPP + +#include +#include +#include + +namespace proxsuite { +namespace osqp { +namespace dense { + +namespace pp = proxsuite::proxqp; +namespace ppd = proxsuite::proxqp::dense; +namespace pod = proxsuite::osqp::dense; + +/// +/// @brief This class defines the API of OSQP solver with dense backend. +/// +template +struct QP : public ppd::QP +{ +public: + /*! + * Class constructors. + */ + using ppd::QP::QP; + /*! + * Solves the QP problem using OSQP algorithm. + */ + void solve() + { + pod::qp_solve( // + this->settings, + this->model, + this->results, + this->work, + this->get_box_constraints(), + this->get_dense_backend(), + this->get_hessian_type(), + this->ruiz); + }; + /*! + * Solves the QP problem using OSQP algorithm using a warm start. + * @param x primal warm start. + * @param y dual equality warm start. + * @param z dual inequality warm start. + */ + void solve(optional> x, + optional> y, + optional> z) + { + ppd::warm_start(x, y, z, this->results, this->settings, this->model); + pod::qp_solve( // + this->settings, + this->model, + this->results, + this->work, + this->get_box_constraints(), + this->get_dense_backend(), + this->get_hessian_type(), + this->ruiz); + }; +}; +/*! + * Solves the QP problem using OSQP algorithm without the need to define a QP + * object, with matrices defined by Dense Eigen matrices. It is possible to set + * up some of the solver parameters (warm start, initial guess option, proximal + * step sizes, absolute and relative accuracies, maximum number of iterations, + * preconditioner execution). There are no box constraints in the model. + * @param H quadratic cost input defining the QP model. + * @param g linear cost input defining the QP model. + * @param A equality constraint matrix input defining the QP model. + * @param b equality constraint vector input defining the QP model. + * @param C inequality constraint matrix input defining the QP model. + * @param l lower inequality constraint vector input defining the QP model. + * @param u upper inequality constraint vector input defining the QP model. + * @param x primal warm start. + * @param y dual equality constraint warm start. + * @param z dual inequality constraint warm start. + * @param verbose if set to true, the solver prints more information about each + * iteration. + * @param compute_preconditioner bool parameter for executing or not the + * preconditioner. + * @param compute_timings boolean parameter for computing the solver timings. + * @param rho proximal step size wrt primal variable. + * @param mu_eq proximal step size wrt equality constrained multiplier. + * @param mu_in proximal step size wrt inequality constrained multiplier. + * @param eps_abs absolute accuracy threshold. + * @param eps_rel relative accuracy threshold. + * @param max_iter maximum number of iteration. + * @param initial_guess initial guess option for warm starting or not the + * initial iterate values. + * @param check_duality_gap If set to true, include the duality gap in absolute + * and relative stopping criteria. + * @param eps_duality_gap_abs absolute accuracy threshold for the duality-gap + * criterion. + * @param eps_duality_gap_rel relative accuracy threshold for the duality-gap + * criterion. + */ +template +proxqp::Results +solve(optional> H, + optional> g, + optional> A, + optional> b, + optional> C, + optional> l, + optional> u, + optional> x = nullopt, + optional> y = nullopt, + optional> z = nullopt, + optional eps_abs = nullopt, + optional eps_rel = nullopt, + optional rho = nullopt, + optional mu_eq = nullopt, + optional mu_in = nullopt, + optional verbose = nullopt, + bool compute_preconditioner = true, + bool compute_timings = false, + optional max_iter = nullopt, + pp::InitialGuessStatus initial_guess = + pp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS, + bool check_duality_gap = false, + optional eps_duality_gap_abs = nullopt, + optional eps_duality_gap_rel = nullopt, + bool primal_infeasibility_solving = false, + optional manual_minimal_H_eigenvalue = nullopt) +{ + isize n(0); + isize n_eq(0); + isize n_in(0); + if (H != nullopt) { + n = H.value().rows(); + } + if (A != nullopt) { + n_eq = A.value().rows(); + } + if (C != nullopt) { + n_in = C.value().rows(); + } + + QP Qp(n, n_eq, n_in, false, DenseBackend::PrimalDualLDLT); + + return proxsuite::common::solve_without_api(Qp, + H, + g, + A, + b, + C, + l, + u, + x, + y, + z, + eps_abs, + eps_rel, + rho, + mu_eq, + mu_in, + verbose, + compute_preconditioner, + compute_timings, + max_iter, + initial_guess, + check_duality_gap, + eps_duality_gap_abs, + eps_duality_gap_rel, + primal_infeasibility_solving, + manual_minimal_H_eigenvalue); +} +/*! + * Solves the QP problem using OSQP algorithm without the need to define a QP + * object, with matrices defined by Dense Eigen matrices. It is possible to set + * up some of the solver parameters (warm start, initial guess option, proximal + * step sizes, absolute and relative accuracies, maximum number of iterations, + * preconditioner execution). + * @param H quadratic cost input defining the QP model. + * @param g linear cost input defining the QP model. + * @param A equality constraint matrix input defining the QP model. + * @param b equality constraint vector input defining the QP model. + * @param C inequality constraint matrix input defining the QP model. + * @param l lower inequality constraint vector input defining the QP model. + * @param u upper inequality constraint vector input defining the QP model. + * @param l_box lower box inequality constraint vector input defining the QP + * model. + * @param u_box upper box inequality constraint vector input defining the QP + * model. + * @param x primal warm start. + * @param y dual equality constraint warm start. + * @param z dual inequality constraint warm start. The upper part must contain a + * warm start for inequality constraints wrt C matrix, whereas the latter wrt + * the box inequalities. + * @param verbose if set to true, the solver prints more information about each + * iteration. + * @param compute_preconditioner bool parameter for executing or not the + * preconditioner. + * @param compute_timings boolean parameter for computing the solver timings. + * @param rho proximal step size wrt primal variable. + * @param mu_eq proximal step size wrt equality constrained multiplier. + * @param mu_in proximal step size wrt inequality constrained multiplier. + * @param eps_abs absolute accuracy threshold. + * @param eps_rel relative accuracy threshold. + * @param max_iter maximum number of iteration. + * @param initial_guess initial guess option for warm starting or not the + * initial iterate values. + * @param check_duality_gap If set to true, include the duality gap in absolute + * and relative stopping criteria. + * @param eps_duality_gap_abs absolute accuracy threshold for the duality-gap + * criterion. + * @param eps_duality_gap_rel relative accuracy threshold for the duality-gap + * criterion. + */ +template +proxqp::Results +solve(optional> H, + optional> g, + optional> A, + optional> b, + optional> C, + optional> l, + optional> u, + optional> l_box, + optional> u_box, + optional> x = nullopt, + optional> y = nullopt, + optional> z = nullopt, + optional eps_abs = nullopt, + optional eps_rel = nullopt, + optional rho = nullopt, + optional mu_eq = nullopt, + optional mu_in = nullopt, + optional verbose = nullopt, + bool compute_preconditioner = true, + bool compute_timings = false, + optional max_iter = nullopt, + pp::InitialGuessStatus initial_guess = + pp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS, + bool check_duality_gap = false, + optional eps_duality_gap_abs = nullopt, + optional eps_duality_gap_rel = nullopt, + bool primal_infeasibility_solving = false, + optional manual_minimal_H_eigenvalue = nullopt) +{ + isize n(0); + isize n_eq(0); + isize n_in(0); + if (H != nullopt) { + n = H.value().rows(); + } + if (A != nullopt) { + n_eq = A.value().rows(); + } + if (C != nullopt) { + n_in = C.value().rows(); + } + + QP Qp(n, n_eq, n_in, true, DenseBackend::PrimalDualLDLT); + + return proxsuite::common::solve_without_api(Qp, + H, + g, + A, + b, + C, + l, + u, + l_box, + u_box, + x, + y, + z, + eps_abs, + eps_rel, + rho, + mu_eq, + mu_in, + verbose, + compute_preconditioner, + compute_timings, + max_iter, + initial_guess, + check_duality_gap, + eps_duality_gap_abs, + eps_duality_gap_rel, + primal_infeasibility_solving, + manual_minimal_H_eigenvalue); +} + +template +bool +operator==(const QP& qp1, const QP& qp2) +{ + return proxsuite::common::is_equal(qp1, qp2); +} + +template +bool +operator!=(const QP& qp1, const QP& qp2) +{ + return !proxsuite::common::is_equal(qp1, qp2); +} + +} // namespace dense +} // namespace osqp +} // namespace proxsuite + +#endif /* end of include guard PROXSUITE_OSQP_DENSE_WRAPPER_HPP */ \ No newline at end of file diff --git a/include/proxsuite/osqp/utils/prints.hpp b/include/proxsuite/osqp/utils/prints.hpp new file mode 100644 index 000000000..1b4bc6ddf --- /dev/null +++ b/include/proxsuite/osqp/utils/prints.hpp @@ -0,0 +1,38 @@ +// +// Copyright (c) 2025 INRIA +// +/** \file */ + +#ifndef PROXSUITE_OSQP_UTILS_PRINTS_HPP +#define PROXSUITE_OSQP_UTILS_PRINTS_HPP + +#include + +namespace proxsuite { +namespace osqp { + +inline void +print_line() +{ + std::string the_line = "-----------------------------------------------------" + "--------------------------------------------\0"; + std::cout << the_line << "\n" << std::endl; +} + +inline void +print_preambule() +{ + print_line(); + std::cout << "OSQP - An operator splitting algorithm for QP programs\n" + << "(c) Paper - Bartolomeo Stellato, Goran Banjac, Paul Goulart, " + "Alberto Bemporad and Stephen Boyd\n" + << "(c) Implementation - Lucas Haubert\n" + << "Inria Paris 2025\n" + << std::endl; + print_line(); +} + +} // end namespace osqp +} // end namespace proxsuite + +#endif /* end of include guard PROXSUITE_OSQP_UTILS_PRINTS_HPP */ diff --git a/include/proxsuite/proxqp/dense/solver.hpp b/include/proxsuite/proxqp/dense/solver.hpp index 6d428ae64..3333c638a 100644 --- a/include/proxsuite/proxqp/dense/solver.hpp +++ b/include/proxsuite/proxqp/dense/solver.hpp @@ -13,6 +13,7 @@ #include "proxsuite/proxqp/dense/linesearch.hpp" #include "proxsuite/proxqp/dense/helpers.hpp" #include "proxsuite/proxqp/dense/utils.hpp" +#include "proxsuite/solvers/common/utils.hpp" #include #include #include @@ -1105,276 +1106,22 @@ qp_solve( // std::cout << "test " << test << std::endl; */ PROXSUITE_EIGEN_MALLOC_NOT_ALLOWED(); + + proxsuite::common::setup_solver(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + dense_backend, + hessian_type, + ruiz, + common::QPSolver::PROXQP); + isize n_constraints(qpmodel.n_in); if (box_constraints) { n_constraints += qpmodel.dim; } - if (qpsettings.compute_timings) { - qpwork.timer.stop(); - qpwork.timer.start(); - } - if (qpsettings.verbose) { - dense::print_setup_header(qpsettings, - qpresults, - qpmodel, - box_constraints, - dense_backend, - hessian_type); - } - // std::cout << "qpwork.dirty " << qpwork.dirty << std::endl; - if (qpwork.dirty) { // the following is used when a solve has already been - // executed (and without any intermediary model update) - switch (qpsettings.initial_guess) { - case InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS: { - qpwork.cleanup(box_constraints); - qpresults.cleanup(qpsettings); - break; - } - case InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT: { - // keep solutions but restart workspace and results - qpwork.cleanup(box_constraints); - qpresults.cold_start(qpsettings); - ruiz.scale_primal_in_place( - { proxsuite::proxqp::from_eigen, qpresults.x }); - ruiz.scale_dual_in_place_eq( - { proxsuite::proxqp::from_eigen, qpresults.y }); - ruiz.scale_dual_in_place_in( - { proxsuite::proxqp::from_eigen, qpresults.z.head(qpmodel.n_in) }); - if (box_constraints) { - ruiz.scale_box_dual_in_place_in( - { proxsuite::proxqp::from_eigen, qpresults.z.tail(qpmodel.dim) }); - } - break; - } - case InitialGuessStatus::NO_INITIAL_GUESS: { - qpwork.cleanup(box_constraints); - qpresults.cleanup(qpsettings); - break; - } - case InitialGuessStatus::WARM_START: { - qpwork.cleanup(box_constraints); - qpresults.cold_start( - qpsettings); // because there was already a solve, - // precond was already computed if set so - ruiz.scale_primal_in_place( - { proxsuite::proxqp::from_eigen, - qpresults - .x }); // it contains the value given in entry for warm start - ruiz.scale_dual_in_place_eq( - { proxsuite::proxqp::from_eigen, qpresults.y }); - ruiz.scale_dual_in_place_in( - { proxsuite::proxqp::from_eigen, qpresults.z.head(qpmodel.n_in) }); - if (box_constraints) { - ruiz.scale_box_dual_in_place_in( - { proxsuite::proxqp::from_eigen, qpresults.z.tail(qpmodel.dim) }); - } - break; - } - case InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT: { - // keep workspace and results solutions except statistics - // std::cout << "i keep previous solution" << std::endl; - qpresults.cleanup_statistics(); - ruiz.scale_primal_in_place( - { proxsuite::proxqp::from_eigen, qpresults.x }); - ruiz.scale_dual_in_place_eq( - { proxsuite::proxqp::from_eigen, qpresults.y }); - ruiz.scale_dual_in_place_in( - { proxsuite::proxqp::from_eigen, qpresults.z.head(qpmodel.n_in) }); - if (box_constraints) { - ruiz.scale_box_dual_in_place_in( - { proxsuite::proxqp::from_eigen, qpresults.z.tail(qpmodel.dim) }); - } - break; - } - } - if (qpsettings.initial_guess != - InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT) { - switch (hessian_type) { - case HessianType::Zero: - break; - case HessianType::Dense: - qpwork.H_scaled = qpmodel.H; - break; - case HessianType::Diagonal: - qpwork.H_scaled = qpmodel.H; - break; - } - qpwork.g_scaled = qpmodel.g; - qpwork.A_scaled = qpmodel.A; - qpwork.b_scaled = qpmodel.b; - qpwork.C_scaled = qpmodel.C; - qpwork.u_scaled = qpmodel.u; - qpwork.l_scaled = qpmodel.l; - proxsuite::proxqp::dense::setup_equilibration( - qpwork, - qpsettings, - box_constraints, - hessian_type, - ruiz, - false); // reuse previous equilibration - proxsuite::proxqp::dense::setup_factorization( - qpwork, qpmodel, qpresults, dense_backend, hessian_type); - } - switch (qpsettings.initial_guess) { - case InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS: { - compute_equality_constrained_initial_guess(qpwork, - qpsettings, - qpmodel, - n_constraints, - dense_backend, - hessian_type, - qpresults); - break; - } - case InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT: { - //!\ TODO in a quicker way - qpwork.n_c = 0; - for (isize i = 0; i < n_constraints; i++) { - if (qpresults.z[i] != 0) { - qpwork.active_inequalities[i] = true; - } else { - qpwork.active_inequalities[i] = false; - } - } - linesearch::active_set_change( - qpmodel, qpresults, dense_backend, n_constraints, qpwork); - break; - } - case InitialGuessStatus::NO_INITIAL_GUESS: { - break; - } - case InitialGuessStatus::WARM_START: { - //!\ TODO in a quicker way - qpwork.n_c = 0; - for (isize i = 0; i < n_constraints; i++) { - if (qpresults.z[i] != 0) { - qpwork.active_inequalities[i] = true; - } else { - qpwork.active_inequalities[i] = false; - } - } - linesearch::active_set_change( - qpmodel, qpresults, dense_backend, n_constraints, qpwork); - break; - } - case InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT: { - // keep workspace and results solutions except statistics - // std::cout << "i use previous solution" << std::endl; - // meaningful for when one wants to warm start with previous result with - // the same QP model - break; - } - } - } else { // the following is used for a first solve after initializing or - // updating the Qp object - switch (qpsettings.initial_guess) { - case InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS: { - proxsuite::proxqp::dense::setup_factorization( - qpwork, qpmodel, qpresults, dense_backend, hessian_type); - compute_equality_constrained_initial_guess(qpwork, - qpsettings, - qpmodel, - n_constraints, - dense_backend, - hessian_type, - qpresults); - break; - } - case InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT: { - //!\ TODO in a quicker way - ruiz.scale_primal_in_place( - { proxsuite::proxqp::from_eigen, - qpresults - .x }); // meaningful for when there is an upate of the model and - // one wants to warm start with previous result - ruiz.scale_dual_in_place_eq( - { proxsuite::proxqp::from_eigen, qpresults.y }); - ruiz.scale_dual_in_place_in( - { proxsuite::proxqp::from_eigen, qpresults.z.head(qpmodel.n_in) }); - if (box_constraints) { - ruiz.scale_box_dual_in_place_in( - { proxsuite::proxqp::from_eigen, qpresults.z.tail(qpmodel.dim) }); - } - setup_factorization( - qpwork, qpmodel, qpresults, dense_backend, hessian_type); - qpwork.n_c = 0; - for (isize i = 0; i < n_constraints; i++) { - if (qpresults.z[i] != 0) { - qpwork.active_inequalities[i] = true; - } else { - qpwork.active_inequalities[i] = false; - } - } - linesearch::active_set_change( - qpmodel, qpresults, dense_backend, n_constraints, qpwork); - break; - } - case InitialGuessStatus::NO_INITIAL_GUESS: { - setup_factorization( - qpwork, qpmodel, qpresults, dense_backend, hessian_type); - break; - } - case InitialGuessStatus::WARM_START: { - //!\ TODO in a quicker way - ruiz.scale_primal_in_place( - { proxsuite::proxqp::from_eigen, qpresults.x }); - ruiz.scale_dual_in_place_eq( - { proxsuite::proxqp::from_eigen, qpresults.y }); - ruiz.scale_dual_in_place_in( - { proxsuite::proxqp::from_eigen, qpresults.z.head(qpmodel.n_in) }); - if (box_constraints) { - ruiz.scale_box_dual_in_place_in( - { proxsuite::proxqp::from_eigen, qpresults.z.tail(qpmodel.dim) }); - } - setup_factorization( - qpwork, qpmodel, qpresults, dense_backend, hessian_type); - qpwork.n_c = 0; - for (isize i = 0; i < n_constraints; i++) { - if (qpresults.z[i] != 0) { - qpwork.active_inequalities[i] = true; - } else { - qpwork.active_inequalities[i] = false; - } - } - linesearch::active_set_change( - qpmodel, qpresults, dense_backend, n_constraints, qpwork); - break; - } - case InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT: { - // std::cout << "i refactorize from previous solution" << std::endl; - ruiz.scale_primal_in_place( - { proxsuite::proxqp::from_eigen, - qpresults - .x }); // meaningful for when there is an upate of the model and - // one wants to warm start with previous result - ruiz.scale_dual_in_place_eq( - { proxsuite::proxqp::from_eigen, qpresults.y }); - ruiz.scale_dual_in_place_in( - { proxsuite::proxqp::from_eigen, qpresults.z.head(qpmodel.n_in) }); - if (box_constraints) { - ruiz.scale_box_dual_in_place_in( - { proxsuite::proxqp::from_eigen, qpresults.z.tail(qpmodel.dim) }); - } - if (qpwork.refactorize) { // refactorization only when one of the - // matrices has changed or one proximal - // parameter has changed - setup_factorization( - qpwork, qpmodel, qpresults, dense_backend, hessian_type); - qpwork.n_c = 0; - for (isize i = 0; i < n_constraints; i++) { - if (qpresults.z[i] != 0) { - qpwork.active_inequalities[i] = true; - } else { - qpwork.active_inequalities[i] = false; - } - } - linesearch::active_set_change( - qpmodel, qpresults, dense_backend, n_constraints, qpwork); - break; - } - } - } - } + T bcl_eta_ext_init = pow(T(0.1), qpsettings.alpha_bcl); T bcl_eta_ext = bcl_eta_ext_init; T bcl_eta_in(1); @@ -1393,170 +1140,55 @@ qp_solve( // T duality_gap(0); T rhs_duality_gap(0); T scaled_eps(qpsettings.eps_abs); + T scaled_eps_rel(qpsettings.eps_rel); for (i64 iter = 0; iter < qpsettings.max_iter; ++iter) { - // compute primal residual - - // PERF: fuse matrix product computations in global_{primal, dual}_residual - global_primal_residual(qpmodel, - qpresults, - qpsettings, - qpwork, - ruiz, - box_constraints, - primal_feasibility_lhs, - primal_feasibility_eq_rhs_0, - primal_feasibility_in_rhs_0, - primal_feasibility_eq_lhs, - primal_feasibility_in_lhs); - - global_dual_residual(qpresults, - qpwork, - qpmodel, - box_constraints, - ruiz, - dual_feasibility_lhs, - dual_feasibility_rhs_0, - dual_feasibility_rhs_1, - dual_feasibility_rhs_3, - rhs_duality_gap, - duality_gap, - hessian_type); - - qpresults.info.pri_res = primal_feasibility_lhs; - qpresults.info.dua_res = dual_feasibility_lhs; - qpresults.info.duality_gap = duality_gap; - T new_bcl_mu_in(qpresults.info.mu_in); T new_bcl_mu_eq(qpresults.info.mu_eq); T new_bcl_mu_in_inv(qpresults.info.mu_in_inv); T new_bcl_mu_eq_inv(qpresults.info.mu_eq_inv); - T rhs_pri(scaled_eps); - if (qpsettings.eps_rel != 0) { - rhs_pri += qpsettings.eps_rel * std::max(primal_feasibility_eq_rhs_0, - primal_feasibility_in_rhs_0); - } - bool is_primal_feasible = primal_feasibility_lhs <= rhs_pri; - - T rhs_dua(qpsettings.eps_abs); - if (qpsettings.eps_rel != 0) { - rhs_dua += - qpsettings.eps_rel * - std::max( - std::max(dual_feasibility_rhs_3, dual_feasibility_rhs_0), - std::max(dual_feasibility_rhs_1, qpwork.dual_feasibility_rhs_2)); + bool is_solved = proxsuite::common::is_solved(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + hessian_type, + ruiz, + common::QPSolver::PROXQP, + primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0, + primal_feasibility_eq_lhs, + primal_feasibility_in_lhs, + primal_feasibility_lhs, + dual_feasibility_lhs, + dual_feasibility_rhs_0, + dual_feasibility_rhs_1, + dual_feasibility_rhs_3, + rhs_duality_gap, + duality_gap, + scaled_eps, + scaled_eps_rel, + iter); + if (is_solved) { + break; } - bool is_dual_feasible = dual_feasibility_lhs <= rhs_dua; - - if (qpsettings.verbose) { - - ruiz.unscale_primal_in_place(VectorViewMut{ from_eigen, qpresults.x }); - ruiz.unscale_dual_in_place_eq( - VectorViewMut{ from_eigen, qpresults.y }); - ruiz.unscale_dual_in_place_in( - VectorViewMut{ from_eigen, qpresults.z.head(qpmodel.n_in) }); - if (box_constraints) { - ruiz.unscale_box_dual_in_place_in( - VectorViewMut{ from_eigen, qpresults.z.tail(qpmodel.dim) }); - } - { - // EigenAllowAlloc _{}; - qpresults.info.objValue = 0; - for (Eigen::Index j = 0; j < qpmodel.dim; ++j) { - qpresults.info.objValue += - 0.5 * (qpresults.x(j) * qpresults.x(j)) * qpmodel.H(j, j); - qpresults.info.objValue += - qpresults.x(j) * T(qpmodel.H.col(j) - .tail(qpmodel.dim - j - 1) - .dot(qpresults.x.tail(qpmodel.dim - j - 1))); - } - qpresults.info.objValue += (qpmodel.g).dot(qpresults.x); - } - std::cout << "\033[1;32m[outer iteration " << iter + 1 << "]\033[0m" - << std::endl; - std::cout << std::scientific << std::setw(2) << std::setprecision(2) - << "| primal residual=" << qpresults.info.pri_res - << " | dual residual=" << qpresults.info.dua_res - << " | duality gap=" << qpresults.info.duality_gap - << " | mu_in=" << qpresults.info.mu_in - << " | rho=" << qpresults.info.rho << std::endl; - ruiz.scale_primal_in_place(VectorViewMut{ from_eigen, qpresults.x }); - ruiz.scale_dual_in_place_eq(VectorViewMut{ from_eigen, qpresults.y }); - ruiz.scale_dual_in_place_in( - VectorViewMut{ from_eigen, qpresults.z.head(qpmodel.n_in) }); - if (box_constraints) { - ruiz.scale_box_dual_in_place_in( - VectorViewMut{ from_eigen, qpresults.z.tail(qpmodel.dim) }); - } - } - if (is_primal_feasible && is_dual_feasible) { - if (qpsettings.check_duality_gap) { - if (std::fabs(qpresults.info.duality_gap) <= - qpsettings.eps_duality_gap_abs + - qpsettings.eps_duality_gap_rel * rhs_duality_gap) { - if (qpsettings.primal_infeasibility_solving && - qpresults.info.status == - QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { - qpresults.info.status = - QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE; - } else { - qpresults.info.status = QPSolverOutput::PROXQP_SOLVED; - } - break; - } - } else { - qpresults.info.status = QPSolverOutput::PROXQP_SOLVED; - break; - } - } qpresults.info.iter_ext += 1; // We start a new external loop update qpwork.x_prev = qpresults.x; qpwork.y_prev = qpresults.y; qpwork.z_prev = qpresults.z; - // primal dual version from gill and robinson - - ruiz.scale_primal_residual_in_place_in( - VectorViewMut{ from_eigen, - qpwork.primal_residual_in_scaled_up.head( - qpmodel.n_in) }); // contains now scaled(Cx) - if (box_constraints) { - ruiz.scale_box_primal_residual_in_place_in( - VectorViewMut{ from_eigen, - qpwork.primal_residual_in_scaled_up.tail( - qpmodel.dim) }); // contains now scaled(x) - } - qpwork.primal_residual_in_scaled_up += - qpwork.z_prev * - qpresults.info.mu_in; // contains now scaled(Cx+z_prev*mu_in) - switch (qpsettings.merit_function_type) { - case MeritFunctionType::GPDAL: - qpwork.primal_residual_in_scaled_up += - (qpsettings.alpha_gpdal - 1.) * qpresults.info.mu_in * qpresults.z; - break; - case MeritFunctionType::PDAL: - break; - } - qpresults.si = qpwork.primal_residual_in_scaled_up; - qpwork.primal_residual_in_scaled_up.head(qpmodel.n_in) -= - qpwork.u_scaled; // contains now scaled(Cx-u+z_prev*mu_in) - qpresults.si.head(qpmodel.n_in) -= - qpwork.l_scaled; // contains now scaled(Cx-l+z_prev*mu_in) - if (box_constraints) { - // qpwork.primal_residual_in_scaled_up.tail(qpmodel.dim) -= - // qpmodel.u_box; // contains now scaled(Cx-u+z_prev*mu_in) - // qpwork.primal_residual_in_scaled_low.tail(qpmodel.dim) -= - // qpmodel.l_box; // contains now scaled(Cx-l+z_prev*mu_in) - - qpwork.primal_residual_in_scaled_up.tail(qpmodel.dim) -= - qpwork.u_box_scaled; // contains now scaled(Cx-u+z_prev*mu_in) - qpresults.si.tail(qpmodel.dim) -= - qpwork.l_box_scaled; // contains now scaled(Cx-l+z_prev*mu_in) - } + proxsuite::common::compute_scaled_primal_residual_ineq( + qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + ruiz, + common::QPSolver::PROXQP); primal_dual_newton_semi_smooth(qpsettings, qpmodel, @@ -1593,77 +1225,30 @@ qp_solve( // scaled_eps = infty_norm(qpwork.rhs.head(qpmodel.dim)) * qpsettings.eps_abs; } + T primal_feasibility_lhs_new(primal_feasibility_lhs); + T dual_feasibility_lhs_new(dual_feasibility_lhs); + proxsuite::common::update_solver_status(qpsettings, + qpmodel, + qpresults, + qpwork, + box_constraints, + hessian_type, + ruiz, + primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0, + primal_feasibility_eq_lhs, + primal_feasibility_in_lhs, + primal_feasibility_lhs_new, + dual_feasibility_lhs_new, + dual_feasibility_rhs_0, + dual_feasibility_rhs_1, + dual_feasibility_rhs_3, + rhs_duality_gap, + duality_gap, + scaled_eps, + scaled_eps_rel); - global_primal_residual(qpmodel, - qpresults, - qpsettings, - qpwork, - ruiz, - box_constraints, - primal_feasibility_lhs_new, - primal_feasibility_eq_rhs_0, - primal_feasibility_in_rhs_0, - primal_feasibility_eq_lhs, - primal_feasibility_in_lhs); - - is_primal_feasible = - primal_feasibility_lhs_new <= - (scaled_eps + qpsettings.eps_rel * std::max(primal_feasibility_eq_rhs_0, - primal_feasibility_in_rhs_0)); - qpresults.info.pri_res = primal_feasibility_lhs_new; - if (is_primal_feasible) { - T dual_feasibility_lhs_new(dual_feasibility_lhs); - - global_dual_residual(qpresults, - qpwork, - qpmodel, - box_constraints, - ruiz, - dual_feasibility_lhs_new, - dual_feasibility_rhs_0, - dual_feasibility_rhs_1, - dual_feasibility_rhs_3, - rhs_duality_gap, - duality_gap, - hessian_type); - qpresults.info.dua_res = dual_feasibility_lhs_new; - qpresults.info.duality_gap = duality_gap; - - is_dual_feasible = - dual_feasibility_lhs_new <= - (qpsettings.eps_abs + - qpsettings.eps_rel * - std::max( - std::max(dual_feasibility_rhs_3, dual_feasibility_rhs_0), - std::max(dual_feasibility_rhs_1, qpwork.dual_feasibility_rhs_2))); - - if (is_dual_feasible) { - if (qpsettings.check_duality_gap) { - if (std::fabs(qpresults.info.duality_gap) <= - qpsettings.eps_duality_gap_abs + - qpsettings.eps_duality_gap_rel * rhs_duality_gap) { - if (qpsettings.primal_infeasibility_solving && - qpresults.info.status == - QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { - qpresults.info.status = - QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE; - } else { - qpresults.info.status = QPSolverOutput::PROXQP_SOLVED; - } - } - } else { - if (qpsettings.primal_infeasibility_solving && - qpresults.info.status == - QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { - qpresults.info.status = - QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE; - } else { - qpresults.info.status = QPSolverOutput::PROXQP_SOLVED; - } - } - } - } if (qpsettings.bcl_update) { bcl_update(qpsettings, qpresults, @@ -1692,23 +1277,6 @@ qp_solve( // } // COLD RESTART - T dual_feasibility_lhs_new(dual_feasibility_lhs); - - global_dual_residual(qpresults, - qpwork, - qpmodel, - box_constraints, - ruiz, - dual_feasibility_lhs_new, - dual_feasibility_rhs_0, - dual_feasibility_rhs_1, - dual_feasibility_rhs_3, - rhs_duality_gap, - duality_gap, - hessian_type); - qpresults.info.dua_res = dual_feasibility_lhs_new; - qpresults.info.duality_gap = duality_gap; - if (primal_feasibility_lhs_new >= primal_feasibility_lhs && dual_feasibility_lhs_new >= dual_feasibility_lhs && qpresults.info.mu_in <= T(1e-5)) { @@ -1746,98 +1314,19 @@ qp_solve( // qpresults.info.mu_in_inv = new_bcl_mu_in_inv; } - ruiz.unscale_primal_in_place(VectorViewMut{ from_eigen, qpresults.x }); - ruiz.unscale_dual_in_place_eq(VectorViewMut{ from_eigen, qpresults.y }); - ruiz.unscale_dual_in_place_in( - VectorViewMut{ from_eigen, qpresults.z.head(qpmodel.n_in) }); - if (box_constraints) { - ruiz.unscale_box_dual_in_place_in( - VectorViewMut{ from_eigen, qpresults.z.tail(qpmodel.dim) }); - } - if (qpsettings.primal_infeasibility_solving && - qpresults.info.status == QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { - ruiz.unscale_primal_residual_in_place_eq( - VectorViewMut{ from_eigen, qpresults.se }); - ruiz.unscale_primal_residual_in_place_in( - VectorViewMut{ from_eigen, qpresults.si.head(qpmodel.n_in) }); - if (box_constraints) { - ruiz.unscale_box_primal_residual_in_place_in( - VectorViewMut{ from_eigen, qpresults.si.tail(qpmodel.dim) }); - } - } - - { - // EigenAllowAlloc _{}; - qpresults.info.objValue = 0; - for (Eigen::Index j = 0; j < qpmodel.dim; ++j) { - qpresults.info.objValue += - 0.5 * (qpresults.x(j) * qpresults.x(j)) * qpmodel.H(j, j); - qpresults.info.objValue += - qpresults.x(j) * T(qpmodel.H.col(j) - .tail(qpmodel.dim - j - 1) - .dot(qpresults.x.tail(qpmodel.dim - j - 1))); - } - qpresults.info.objValue += (qpmodel.g).dot(qpresults.x); - } - + proxsuite::common::unscale_solver( + qpsettings, qpmodel, qpresults, box_constraints, ruiz); + proxsuite::common::compute_objective(qpmodel, qpresults); if (qpsettings.compute_timings) { - qpresults.info.solve_time = qpwork.timer.elapsed().user; // in microseconds - qpresults.info.run_time = - qpresults.info.solve_time + qpresults.info.setup_time; + proxsuite::common::compute_timings(qpresults, qpwork); } if (qpsettings.verbose) { - std::cout << "-------------------SOLVER STATISTICS-------------------" - << std::endl; - std::cout << "outer iter: " << qpresults.info.iter_ext << std::endl; - std::cout << "total iter: " << qpresults.info.iter << std::endl; - std::cout << "mu updates: " << qpresults.info.mu_updates << std::endl; - std::cout << "rho updates: " << qpresults.info.rho_updates << std::endl; - std::cout << "objective: " << qpresults.info.objValue << std::endl; - switch (qpresults.info.status) { - case QPSolverOutput::PROXQP_SOLVED: { - std::cout << "status: " - << "Solved" << std::endl; - break; - } - case QPSolverOutput::PROXQP_MAX_ITER_REACHED: { - std::cout << "status: " - << "Maximum number of iterations reached" << std::endl; - break; - } - case QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE: { - std::cout << "status: " - << "Primal infeasible" << std::endl; - break; - } - case QPSolverOutput::PROXQP_DUAL_INFEASIBLE: { - std::cout << "status: " - << "Dual infeasible" << std::endl; - break; - } - case QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE: { - std::cout << "status: " - << "Solved closest primal feasible" << std::endl; - break; - } - case QPSolverOutput::PROXQP_NOT_RUN: { - std::cout << "status: " - << "Solver not run" << std::endl; - break; - } - } - - if (qpsettings.compute_timings) - std::cout << "run time [μs]: " << qpresults.info.solve_time << std::endl; - std::cout << "--------------------------------------------------------" - << std::endl; + proxsuite::common::print_solver_statistics( + qpsettings, qpresults, common::QPSolver::PROXQP); } - qpwork.dirty = true; - qpwork.is_initialized = true; // necessary because we call workspace cleanup - assert(!std::isnan(qpresults.info.pri_res)); - assert(!std::isnan(qpresults.info.dua_res)); - assert(!std::isnan(qpresults.info.duality_gap)); + proxsuite::common::prepare_next_solve(qpresults, qpwork); PROXSUITE_EIGEN_MALLOC_ALLOWED(); } diff --git a/include/proxsuite/proxqp/dense/utils.hpp b/include/proxsuite/proxqp/dense/utils.hpp index 6d9c62857..3eede862a 100644 --- a/include/proxsuite/proxqp/dense/utils.hpp +++ b/include/proxsuite/proxqp/dense/utils.hpp @@ -28,100 +28,6 @@ namespace proxsuite { namespace proxqp { namespace dense { -template -void -print_setup_header(const Settings& settings, - const Results& results, - const Model& model, - const bool box_constraints, - const DenseBackend& dense_backend, - const HessianType& hessian_type) -{ - - proxsuite::proxqp::print_preambule(); - - // Print variables and constraints - std::cout << "problem: " << std::noshowpos << std::endl; - std::cout << " variables n = " << model.dim - << ", equality constraints n_eq = " << model.n_eq << ",\n" - << " inequality constraints n_in = " << model.n_in - << std::endl; - - // Print Settings - std::cout << "settings: " << std::endl; - std::cout << " backend = dense," << std::endl; - std::cout << " eps_abs = " << settings.eps_abs - << " eps_rel = " << settings.eps_rel << std::endl; - std::cout << " eps_prim_inf = " << settings.eps_primal_inf - << ", eps_dual_inf = " << settings.eps_dual_inf << "," << std::endl; - - std::cout << " rho = " << results.info.rho - << ", mu_eq = " << results.info.mu_eq - << ", mu_in = " << results.info.mu_in << "," << std::endl; - std::cout << " max_iter = " << settings.max_iter - << ", max_iter_in = " << settings.max_iter_in << "," << std::endl; - if (box_constraints) { - std::cout << " box constraints: on, " << std::endl; - } else { - std::cout << " box constraints: off, " << std::endl; - } - switch (dense_backend) { - case DenseBackend::PrimalDualLDLT: - std::cout << " dense backend: PrimalDualLDLT, " << std::endl; - break; - case DenseBackend::PrimalLDLT: - std::cout << " dense backend: PrimalLDLT, " << std::endl; - break; - case DenseBackend::Automatic: - break; - } - switch (hessian_type) { - case HessianType::Dense: - std::cout << " problem type: Quadratic Program, " << std::endl; - break; - case HessianType::Zero: - std::cout << " problem type: Linear Program, " << std::endl; - break; - case HessianType::Diagonal: - std::cout - << " problem type: Quadratic Program with diagonal Hessian, " - << std::endl; - break; - } - if (settings.compute_preconditioner) { - std::cout << " scaling: on, " << std::endl; - } else { - std::cout << " scaling: off, " << std::endl; - } - if (settings.compute_timings) { - std::cout << " timings: on, " << std::endl; - } else { - std::cout << " timings: off, " << std::endl; - } - switch (settings.initial_guess) { - case InitialGuessStatus::WARM_START: - std::cout << " initial guess: warm start. \n" << std::endl; - break; - case InitialGuessStatus::NO_INITIAL_GUESS: - std::cout << " initial guess: no initial guess. \n" << std::endl; - break; - case InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT: - std::cout - << " initial guess: warm start with previous result. \n" - << std::endl; - break; - case InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT: - std::cout - << " initial guess: cold start with previous result. \n" - << std::endl; - break; - case InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS: - std::cout - << " initial guess: equality constrained initial guess. \n" - << std::endl; - } -} - /*! * Save a matrix into a CSV format. Used for debug purposes. * diff --git a/include/proxsuite/proxqp/dense/workspace.hpp b/include/proxsuite/proxqp/dense/workspace.hpp index 2a4d77b61..31d5ebe4b 100644 --- a/include/proxsuite/proxqp/dense/workspace.hpp +++ b/include/proxsuite/proxqp/dense/workspace.hpp @@ -60,6 +60,15 @@ struct Workspace VecBool active_set_low; VecBool active_inequalities; + //// OSQP variables + Vec zeta_eq; + Vec zeta_in; + Vec nu_eq; + Vec nu_in; + + VecBool active_set_low_eq; + VecBool active_set_up_eq; + //// First order residuals for line search Vec Hdx; @@ -82,6 +91,7 @@ struct Workspace T alpha; Vec dual_residual_scaled; + Vec primal_residual_scaled; Vec primal_residual_in_scaled_up; Vec primal_residual_in_scaled_up_plus_alphaCdx; @@ -93,6 +103,20 @@ struct Workspace bool refactorize; bool proximal_parameter_update; bool is_initialized; + bool is_first_solve; + + ///// Timers for update_mu in OSQP + Timer timer_factorization_complete_kkt; + Timer timer_between_updates; + T factorization_time_complete_kkt; + T time_since_last_update_mu; + + ///// Fixed number iterations approach in mu update in OSQP + isize last_iteration_update_mu; + + ///// Timing polishing in OSQP + Timer timer_polish; + T time_polishing; sparse::isize n_c; // final number of active inequalities /*! @@ -116,6 +140,10 @@ struct Workspace , l_scaled(n_in) , x_prev(dim) , y_prev(n_eq) + , zeta_eq(n_eq) + , nu_eq(n_eq) + , active_set_low_eq(n_eq) + , active_set_up_eq(n_eq) , Hdx(dim) , Adx(n_eq) , dual_residual_scaled(dim) @@ -125,6 +153,7 @@ struct Workspace , refactorize(false) , proximal_parameter_update(false) , is_initialized(false) + , is_first_solve(true) { if (box_constraints) { @@ -207,9 +236,12 @@ struct Workspace active_set_low.resize(n_in + dim); active_inequalities.resize(n_in + dim); active_part_z.resize(n_in + dim); + zeta_in.resize(n_in + dim); + nu_in.resize(n_in + dim); dw_aug.resize(dim + n_eq + n_in + dim); rhs.resize(dim + n_eq + n_in + dim); err.resize(dim + n_eq + n_in + dim); + primal_residual_scaled.resize(n_eq + n_in + dim); primal_residual_in_scaled_up.resize(dim + n_in); primal_residual_in_scaled_up_plus_alphaCdx.resize(dim + n_in); primal_residual_in_scaled_low_plus_alphaCdx.resize(dim + n_in); @@ -282,9 +314,12 @@ struct Workspace active_set_low.resize(n_in); active_inequalities.resize(n_in); active_part_z.resize(n_in); + zeta_in.resize(n_in); + nu_in.resize(n_in); dw_aug.resize(dim + n_eq + n_in); rhs.resize(dim + n_eq + n_in); err.resize(dim + n_eq + n_in); + primal_residual_scaled.resize(n_eq + n_in); primal_residual_in_scaled_up.resize(n_in); primal_residual_in_scaled_up_plus_alphaCdx.resize(n_in); primal_residual_in_scaled_low_plus_alphaCdx.resize(n_in); @@ -302,6 +337,10 @@ struct Workspace x_prev.setZero(); y_prev.setZero(); z_prev.setZero(); + zeta_eq.setZero(); + zeta_in.setZero(); + nu_eq.setZero(); + nu_in.setZero(); kkt.setZero(); Hdx.setZero(); Cdx.setZero(); @@ -317,12 +356,19 @@ struct Workspace alpha = 1.; dual_residual_scaled.setZero(); + primal_residual_scaled.setZero(); primal_residual_in_scaled_up.setZero(); primal_residual_in_scaled_up_plus_alphaCdx.setZero(); primal_residual_in_scaled_low_plus_alphaCdx.setZero(); CTz.setZero(); n_c = 0; + + factorization_time_complete_kkt = 0.; + time_since_last_update_mu = 0.; + last_iteration_update_mu = 0; + + time_polishing = 0.; } /*! * Clean-ups solver's workspace. @@ -338,6 +384,10 @@ struct Workspace b_scaled.setZero(); u_scaled.setZero(); l_scaled.setZero(); + zeta_eq.setZero(); + zeta_in.setZero(); + nu_eq.setZero(); + nu_in.setZero(); Hdx.setZero(); Cdx.setZero(); Adx.setZero(); @@ -349,6 +399,7 @@ struct Workspace alpha = 1.; dual_residual_scaled.setZero(); + primal_residual_scaled.setZero(); primal_residual_in_scaled_up.setZero(); primal_residual_in_scaled_up_plus_alphaCdx.setZero(); @@ -374,6 +425,11 @@ struct Workspace proximal_parameter_update = false; is_initialized = false; n_c = 0; + + time_since_last_update_mu = 0.; + last_iteration_update_mu = 0; + + time_polishing = 0.; } }; } // namespace dense diff --git a/include/proxsuite/proxqp/dense/wrapper.hpp b/include/proxsuite/proxqp/dense/wrapper.hpp index 6e0c1a066..b34384042 100644 --- a/include/proxsuite/proxqp/dense/wrapper.hpp +++ b/include/proxsuite/proxqp/dense/wrapper.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include namespace proxsuite { @@ -128,6 +129,13 @@ struct QP Workspace work; preconditioner::RuizEquilibration ruiz; + /*! + Getters + */ + DenseBackend get_dense_backend() const { return dense_backend; } + bool get_box_constraints() const { return box_constraints; } + HessianType get_hessian_type() const { return hessian_type; } + /*! * Default constructor using QP model dimensions. * @param _dim primal variable dimension. @@ -1041,49 +1049,33 @@ solve( } QP Qp(n, n_eq, n_in, false, DenseBackend::PrimalDualLDLT); - Qp.settings.initial_guess = initial_guess; - Qp.settings.check_duality_gap = check_duality_gap; - - if (eps_abs != nullopt) { - Qp.settings.eps_abs = eps_abs.value(); - } - if (eps_rel != nullopt) { - Qp.settings.eps_rel = eps_rel.value(); - } - if (verbose != nullopt) { - Qp.settings.verbose = verbose.value(); - } - if (max_iter != nullopt) { - Qp.settings.max_iter = max_iter.value(); - } - if (eps_duality_gap_abs != nullopt) { - Qp.settings.eps_duality_gap_abs = eps_duality_gap_abs.value(); - } - if (eps_duality_gap_rel != nullopt) { - Qp.settings.eps_duality_gap_rel = eps_duality_gap_rel.value(); - } - Qp.settings.compute_timings = compute_timings; - Qp.settings.primal_infeasibility_solving = primal_infeasibility_solving; - if (manual_minimal_H_eigenvalue != nullopt) { - Qp.init(H, - g, - A, - b, - C, - l, - u, - compute_preconditioner, - rho, - mu_eq, - mu_in, - manual_minimal_H_eigenvalue.value()); - } else { - Qp.init( - H, g, A, b, C, l, u, compute_preconditioner, rho, mu_eq, mu_in, nullopt); - } - Qp.solve(x, y, z); - return Qp.results; + return proxsuite::common::solve_without_api(Qp, + H, + g, + A, + b, + C, + l, + u, + x, + y, + z, + eps_abs, + eps_rel, + rho, + mu_eq, + mu_in, + verbose, + compute_preconditioner, + compute_timings, + max_iter, + initial_guess, + check_duality_gap, + eps_duality_gap_abs, + eps_duality_gap_rel, + primal_infeasibility_solving, + manual_minimal_H_eigenvalue); } /*! * Solves the QP problem using PROXQP algorithm without the need to define a QP @@ -1173,80 +1165,49 @@ solve( } QP Qp(n, n_eq, n_in, true, DenseBackend::PrimalDualLDLT); - Qp.settings.initial_guess = initial_guess; - Qp.settings.check_duality_gap = check_duality_gap; - - if (eps_abs != nullopt) { - Qp.settings.eps_abs = eps_abs.value(); - } - if (eps_rel != nullopt) { - Qp.settings.eps_rel = eps_rel.value(); - } - if (verbose != nullopt) { - Qp.settings.verbose = verbose.value(); - } - if (max_iter != nullopt) { - Qp.settings.max_iter = max_iter.value(); - } - if (eps_duality_gap_abs != nullopt) { - Qp.settings.eps_duality_gap_abs = eps_duality_gap_abs.value(); - } - if (eps_duality_gap_rel != nullopt) { - Qp.settings.eps_duality_gap_rel = eps_duality_gap_rel.value(); - } - Qp.settings.compute_timings = compute_timings; - Qp.settings.primal_infeasibility_solving = primal_infeasibility_solving; - if (manual_minimal_H_eigenvalue != nullopt) { - Qp.init(H, - g, - A, - b, - C, - l, - u, - l_box, - u_box, - compute_preconditioner, - rho, - mu_eq, - mu_in, - manual_minimal_H_eigenvalue.value()); - } else { - Qp.init(H, - g, - A, - b, - C, - l, - u, - l_box, - u_box, - compute_preconditioner, - rho, - mu_eq, - mu_in, - nullopt); - } - Qp.solve(x, y, z); - return Qp.results; + return proxsuite::common::solve_without_api(Qp, + H, + g, + A, + b, + C, + l, + u, + l_box, + u_box, + x, + y, + z, + eps_abs, + eps_rel, + rho, + mu_eq, + mu_in, + verbose, + compute_preconditioner, + compute_timings, + max_iter, + initial_guess, + check_duality_gap, + eps_duality_gap_abs, + eps_duality_gap_rel, + primal_infeasibility_solving, + manual_minimal_H_eigenvalue); } template bool operator==(const QP& qp1, const QP& qp2) { - bool value = qp1.model == qp2.model && qp1.settings == qp2.settings && - qp1.results == qp2.results && - qp1.is_box_constrained() == qp2.is_box_constrained(); - return value; + return proxsuite::common::is_equal(qp1, qp2); } template bool operator!=(const QP& qp1, const QP& qp2) { - return !(qp1 == qp2); + return !proxsuite::common::is_equal(qp1, qp2); } ///// BatchQP object diff --git a/include/proxsuite/proxqp/results.hpp b/include/proxsuite/proxqp/results.hpp index 987d694e7..0b711dc17 100644 --- a/include/proxsuite/proxqp/results.hpp +++ b/include/proxsuite/proxqp/results.hpp @@ -42,6 +42,15 @@ struct Info sparse::isize rho_updates; QPSolverOutput status; + ///// polishing osqp + PolishStatus polish_status; + + bool admm_solved_at_init; + bool resumed_admm; + bool tried_high_accuracy; + + isize polish_calls; + //// timings T setup_time; T solve_time; @@ -139,6 +148,11 @@ struct Results info.duality_gap = 0.; info.iterative_residual = 0.; info.status = QPSolverOutput::PROXQP_NOT_RUN; + info.polish_status = PolishStatus::POLISH_NOT_RUN; + info.admm_solved_at_init = false; + info.resumed_admm = false; + info.tried_high_accuracy = false; + info.polish_calls = 0; info.sparse_backend = SparseBackend::Automatic; info.minimal_H_eigenvalue_estimate = 0.; } @@ -169,7 +183,13 @@ struct Results info.dua_res = 0.; info.duality_gap = 0.; info.iterative_residual = 0.; - info.status = QPSolverOutput::PROXQP_MAX_ITER_REACHED; + info.status = + QPSolverOutput::PROXQP_MAX_ITER_REACHED; // TODO: PROXQP_NOT_RUN instead ? + info.polish_status = PolishStatus::POLISH_NOT_RUN; + info.admm_solved_at_init = false; + info.resumed_admm = false; + info.tried_high_accuracy = false; + info.polish_calls = 0; info.sparse_backend = SparseBackend::Automatic; } void cold_start(optional> settings = nullopt) @@ -208,18 +228,24 @@ bool operator==(const Info& info1, const Info& info2) { bool value = - info1.mu_eq == info2.mu_eq && info1.mu_eq_inv == info2.mu_eq_inv && - info1.mu_in == info2.mu_in && info1.mu_in_inv == info2.mu_in_inv && - info1.rho == info2.rho && info1.nu == info2.nu && - info1.iter == info2.iter && info1.iter_ext == info2.iter_ext && - info1.mu_updates == info2.mu_updates && - info1.rho_updates == info2.rho_updates && info1.status == info2.status && - info1.setup_time == info2.setup_time && - info1.solve_time == info2.solve_time && info1.run_time == info2.run_time && - info1.objValue == info2.objValue && info1.pri_res == info2.pri_res && - info1.dua_res == info2.dua_res && info1.duality_gap == info2.duality_gap && - info1.duality_gap == info2.duality_gap && - info1.minimal_H_eigenvalue_estimate == info2.minimal_H_eigenvalue_estimate; + info1.mu_eq == info2.mu_eq&& info1.mu_eq_inv == + info2.mu_eq_inv&& info1.mu_in == info2.mu_in&& info1.mu_in_inv == + info2.mu_in_inv&& info1.rho == info2.rho&& info1.nu == + info2.nu&& info1.iter == info2.iter&& info1.iter_ext == + info2.iter_ext&& info1.mu_updates == info2.mu_updates&& info1.rho_updates == + info2.rho_updates&& info1.status == info2.status&& info1.polish_status == + info2.polish_status&& info1.admm_solved_at_init == + info2.admm_solved_at_init&& info1.resumed_admm == + info2.resumed_admm&& info1.tried_high_accuracy = + info2.tried_high_accuracy && info1.setup_time == info2.setup_time && + info1.solve_time && info1.polish_calls == info2.polish_calls && + info2.solve_time && info1.run_time == info2.run_time && + info1.objValue == info2.objValue && info1.pri_res == info2.pri_res && + info1.dua_res == info2.dua_res && + info1.duality_gap == info2.duality_gap && + info1.duality_gap == info2.duality_gap && + info1.minimal_H_eigenvalue_estimate == + info2.minimal_H_eigenvalue_estimate; return value; } diff --git a/include/proxsuite/proxqp/settings.hpp b/include/proxsuite/proxqp/settings.hpp index f455238b4..73dd4a438 100644 --- a/include/proxsuite/proxqp/settings.hpp +++ b/include/proxsuite/proxqp/settings.hpp @@ -51,6 +51,12 @@ enum struct EigenValueEstimateMethodOption // watch out, the last option is only available for dense // matrices! }; +// Choice of approach for the iteraion condition to update mu in OSQP. +enum struct UpdateMuIterationCriteria +{ + FactorizationTime, // Time spent since last update wrt factorization time. + FixedNumberIterations // Fixed number of iteration to outpass. +}; inline std::ostream& operator<<(std::ostream& os, const SparseBackend& sparse_backend) { @@ -95,13 +101,23 @@ struct Settings T alpha_bcl; T beta_bcl; + T alpha_osqp; + T refactor_dual_feasibility_threshold; T refactor_rho_threshold; T mu_min_eq; - T mu_min_in; + T mu_max_eq; // for osqp + T mu_min_in; // osqp 1e-6 + T mu_max_in; // for osqp + T mu_max_eq_inv; - T mu_max_in_inv; + T mu_min_eq_inv; // for osqp + T mu_max_in_inv; // osqp 1e6 + T mu_min_in_inv; // for osqp + + T mu_min_in_osqp; + T mu_max_in_inv_osqp; T mu_update_factor; T mu_update_inv_factor; @@ -110,9 +126,17 @@ struct Settings T cold_reset_mu_in; T cold_reset_mu_eq_inv; T cold_reset_mu_in_inv; + + T cold_reset_mu_eq_osqp; + T cold_reset_mu_in_osqp; + T cold_reset_mu_eq_inv_osqp; + T cold_reset_mu_in_inv_osqp; + T eps_abs; T eps_rel; + bool high_accuracy; + isize max_iter; isize max_iter_in; isize safe_guard; @@ -129,6 +153,19 @@ struct Settings T eps_duality_gap_abs; T eps_duality_gap_rel; + bool update_mu; + T threshold_ratio_update_mu; + T threshold_ratio_update_mu_inv; + T percentage_factorization_time_update_mu; + UpdateMuIterationCriteria update_mu_iteration_criteria; + isize interval_update_mu; + + bool polish; + isize polish_refine_iter; + T delta; + bool resume_admm; + bool try_high_accuracy; + isize preconditioner_max_iter; T preconditioner_accuracy; T eps_primal_inf; @@ -148,16 +185,26 @@ struct Settings * @param default_mu_in default mu_in parameter of result class * @param alpha_bcl alpha parameter of the BCL algorithm. * @param beta_bcl beta parameter of the BCL algorithm. + * @param alpha_osqp alpha parameter in the ADMM step in OSQP. * @param refactor_dual_feasibility_threshold threshold above which * refactorization is performed to change rho parameter. * @param refactor_rho_threshold new rho parameter used if the * refactor_dual_feasibility_threshold_ condition has been satisfied. * @param mu_min_eq minimal authorized value for mu_eq. + * @param mu_max_eq maximal authorized value for mu_eq. * @param mu_min_in minimal authorized value for mu_in. + * @param mu_max_in maximal authorized value for mu_in. * @param mu_max_eq_inv maximal authorized value for the inverse of * mu_eq_inv. + * @param mu_min_eq_inv minimal authorized value for the inverse of + * mu_eq_inv. * @param mu_max_in_inv maximal authorized value for the inverse of * mu_in_inv. + * @param mu_min_in_inv minimal authorized value for the inverse of + * mu_in_inv. + * @param mu_min_in_osqp minimal authorized value for mu_in in osqp. + * @param mu_max_in_inv_osqp maximal authorized value for the inverse of mu_in + * in osqp. * @param mu_update_factor update factor used for updating mu_eq and mu_in. * @param mu_update_inv_factor update factor used for updating mu_eq_inv and * mu_in_inv. @@ -165,8 +212,16 @@ struct Settings * @param cold_reset_mu_in value used for cold restarting mu_in. * @param cold_reset_mu_eq_inv value used for cold restarting mu_eq_inv. * @param cold_reset_mu_in_inv value used for cold restarting mu_in_inv. + * @param cold_reset_mu_eq_osqp value used for cold restarting mu_eq in osqp. + * @param cold_reset_mu_in_osqp value used for cold restarting mu_in in osqp. + * @param cold_reset_mu_eq_inv_osqp value used for cold restarting mu_eq_inv + * in osqp. + * @param cold_reset_mu_in_inv_osqp value used for cold restarting mu_in_inv + * in osqp. * @param eps_abs asbolute stopping criterion of the solver. * @param eps_rel relative stopping criterion of the solver. + * @param high_accuracy if set to true, epsilon for ADMM set to 1e-5 instead + * of 1e-3 in osqp. * @param max_iter maximal number of authorized iteration. * @param max_iter_in maximal number of authorized iterations for an inner * loop. @@ -189,6 +244,28 @@ struct Settings * included in the stopping criterion. * @param eps_duality_gap_abs absolute duality-gap stopping criterion. * @param eps_duality_gap_rel relative duality-gap stopping criterion. + * @param update_mu If set to true, the proximal parameters in OSQP are + * updated during the solve. + * @param threshold_ratio_update_mu update mu if below the scaled ratio + * between primal and dual residuals. + * @param threshold_ratio_update_mu_inv update mu if above the scaled ratio + * between primal and dual residuals. + * @param percentage_factorization_time_update_mu percentage of the + * factorization time of the complete KKT matrix in OSQP involved in the + * update of mu. + * @param update_mu_iteration_criteria choose wether we potnetially update mu + * after a fixed amount of iterations or some percentage of factorization + * time. + * @param interval_update_mu minimum number of ADMM iterations between two mu + * updates in OSQP if iteration based criteria + * @param polish if set to true, performs solution polishing in OSQP + * @param polish_refine_iter number of iterative refinements in polishing in + * OSQP + * @param delta regularization parameter in solution polishing in OSQP + * @param resume_admm if set to true, resumes to ADMM iterations after polish + * failed to found active sets or provide a better solution. + * @param try_high_accuracy if set to true, resume ADMM after polishing failed + * at precision 1e-3, then try at ADMM precision 1e-5. * @param preconditioner_max_iter maximal number of authorized iterations for * the preconditioner. * @param preconditioner_accuracy accuracy level of the preconditioner. @@ -216,20 +293,32 @@ struct Settings T default_mu_in = 1.E-1, T alpha_bcl = 0.1, T beta_bcl = 0.9, + T alpha_osqp = 1.6, T refactor_dual_feasibility_threshold = 1e-2, T refactor_rho_threshold = 1e-7, T mu_min_eq = 1e-9, + T mu_max_eq = 1e3, // for osqp T mu_min_in = 1e-8, + T mu_max_in = 1e6, // for osqp T mu_max_eq_inv = 1e9, + T mu_min_eq_inv = 1e-3, // for osqp T mu_max_in_inv = 1e8, + T mu_min_in_inv = 1e-6, // for osqp + T mu_min_in_osqp = 1e-6, + T mu_max_in_inv_osqp = 1e6, T mu_update_factor = 0.1, T mu_update_inv_factor = 10, T cold_reset_mu_eq = 1. / 1.1, T cold_reset_mu_in = 1. / 1.1, T cold_reset_mu_eq_inv = 1.1, T cold_reset_mu_in_inv = 1.1, + T cold_reset_mu_eq_osqp = 1.E-2, // default + T cold_reset_mu_in_osqp = 1.E1, // default + T cold_reset_mu_eq_inv_osqp = 1.E2, // default + T cold_reset_mu_in_inv_osqp = 1.E-1, // default T eps_abs = 1.e-5, T eps_rel = 0, + bool high_accuracy = false, isize max_iter = 10000, isize max_iter_in = 1500, isize safe_guard = 1.E4, @@ -247,6 +336,18 @@ struct Settings bool check_duality_gap = false, T eps_duality_gap_abs = 1.e-4, T eps_duality_gap_rel = 0, + bool update_mu = true, + T threshold_ratio_update_mu = 5.0, + T threshold_ratio_update_mu_inv = 0.2, + T percentage_factorization_time_update_mu = 0.4, + UpdateMuIterationCriteria update_mu_iteration_criteria = + UpdateMuIterationCriteria::FixedNumberIterations, + isize interval_update_mu = 50, + bool polish = true, + isize polish_refine_iter = 3, + T delta = 1.e-6, + bool resume_admm = true, + bool try_high_accuracy = true, isize preconditioner_max_iter = 10, T preconditioner_accuracy = 1.e-3, T eps_primal_inf = 1.E-4, @@ -262,20 +363,32 @@ struct Settings , default_mu_in(default_mu_in) , alpha_bcl(alpha_bcl) , beta_bcl(beta_bcl) + , alpha_osqp(alpha_osqp) , refactor_dual_feasibility_threshold(refactor_dual_feasibility_threshold) , refactor_rho_threshold(refactor_rho_threshold) , mu_min_eq(mu_min_eq) + , mu_max_eq(mu_max_eq) , mu_min_in(mu_min_in) + , mu_max_in(mu_max_in) , mu_max_eq_inv(mu_max_eq_inv) + , mu_min_eq_inv(mu_min_eq_inv) , mu_max_in_inv(mu_max_in_inv) + , mu_min_in_inv(mu_min_in_inv) + , mu_min_in_osqp(mu_min_in_osqp) + , mu_max_in_inv_osqp(mu_max_in_inv_osqp) , mu_update_factor(mu_update_factor) , mu_update_inv_factor(mu_update_inv_factor) , cold_reset_mu_eq(cold_reset_mu_eq) , cold_reset_mu_in(cold_reset_mu_in) , cold_reset_mu_eq_inv(cold_reset_mu_eq_inv) , cold_reset_mu_in_inv(cold_reset_mu_in_inv) + , cold_reset_mu_eq_osqp(cold_reset_mu_eq_osqp) + , cold_reset_mu_in_osqp(cold_reset_mu_in_osqp) + , cold_reset_mu_eq_inv_osqp(cold_reset_mu_eq_inv_osqp) + , cold_reset_mu_in_inv_osqp(cold_reset_mu_in_inv_osqp) , eps_abs(eps_abs) , eps_rel(eps_rel) + , high_accuracy(high_accuracy) , max_iter(max_iter) , max_iter_in(max_iter_in) , safe_guard(safe_guard) @@ -289,6 +402,18 @@ struct Settings , check_duality_gap(check_duality_gap) , eps_duality_gap_abs(eps_duality_gap_abs) , eps_duality_gap_rel(eps_duality_gap_rel) + , update_mu(update_mu) + , threshold_ratio_update_mu(threshold_ratio_update_mu) + , threshold_ratio_update_mu_inv(threshold_ratio_update_mu_inv) + , percentage_factorization_time_update_mu( + percentage_factorization_time_update_mu) + , update_mu_iteration_criteria(update_mu_iteration_criteria) + , interval_update_mu(interval_update_mu) + , polish(polish) + , polish_refine_iter(polish_refine_iter) + , delta(delta) + , resume_admm(resume_admm) + , try_high_accuracy(try_high_accuracy) , preconditioner_max_iter(preconditioner_max_iter) , preconditioner_accuracy(preconditioner_accuracy) , eps_primal_inf(eps_primal_inf) @@ -325,21 +450,38 @@ operator==(const Settings& settings1, const Settings& settings2) settings1.default_mu_in == settings2.default_mu_in && settings1.alpha_bcl == settings2.alpha_bcl && settings1.alpha_bcl == settings2.alpha_bcl && + settings1.beta_bcl == settings2.beta_bcl && + settings1.beta_bcl == settings2.beta_bcl && + settings1.alpha_osqp == settings2.alpha_osqp && + settings1.alpha_osqp == settings2.alpha_osqp && settings1.refactor_dual_feasibility_threshold == settings2.refactor_dual_feasibility_threshold && settings1.refactor_rho_threshold == settings2.refactor_rho_threshold && settings1.mu_min_eq == settings2.mu_min_eq && + settings1.mu_max_eq == settings2.mu_max_eq && settings1.mu_min_in == settings2.mu_min_in && + settings1.mu_max_in == settings2.mu_max_in && settings1.mu_max_eq_inv == settings2.mu_max_eq_inv && + settings1.mu_min_eq_inv == settings2.mu_min_eq_inv && settings1.mu_max_in_inv == settings2.mu_max_in_inv && + settings1.mu_min_in_inv == settings2.mu_min_in_inv && + settings1.mu_min_in_osqp == settings2.mu_min_in_osqp && + settings1.mu_max_in_inv_osqp == settings2.mu_max_in_inv_osqp && settings1.mu_update_factor == settings2.mu_update_factor && settings1.mu_update_factor == settings2.mu_update_factor && settings1.cold_reset_mu_eq == settings2.cold_reset_mu_eq && settings1.cold_reset_mu_in == settings2.cold_reset_mu_in && settings1.cold_reset_mu_eq_inv == settings2.cold_reset_mu_eq_inv && settings1.cold_reset_mu_in_inv == settings2.cold_reset_mu_in_inv && + settings1.cold_reset_mu_eq_osqp == settings2.cold_reset_mu_eq_osqp && + settings1.cold_reset_mu_in_osqp == settings2.cold_reset_mu_in_osqp && + settings1.cold_reset_mu_eq_inv_osqp == + settings2.cold_reset_mu_eq_inv_osqp && + settings1.cold_reset_mu_in_inv_osqp == + settings2.cold_reset_mu_in_inv_osqp && settings1.eps_abs == settings2.eps_abs && settings1.eps_rel == settings2.eps_rel && + settings1.high_accuracy == settings2.high_accuracy && settings1.max_iter == settings2.max_iter && settings1.max_iter_in == settings2.max_iter_in && settings1.safe_guard == settings2.safe_guard && @@ -353,6 +495,21 @@ operator==(const Settings& settings1, const Settings& settings2) settings1.check_duality_gap == settings2.check_duality_gap && settings1.eps_duality_gap_abs == settings2.eps_duality_gap_abs && settings1.eps_duality_gap_rel == settings2.eps_duality_gap_rel && + settings1.update_mu == settings2.update_mu && + settings1.threshold_ratio_update_mu == + settings2.threshold_ratio_update_mu && + settings1.threshold_ratio_update_mu_inv == + settings2.threshold_ratio_update_mu_inv && + settings1.percentage_factorization_time_update_mu == + settings2.percentage_factorization_time_update_mu && + settings1.update_mu_iteration_criteria == + settings2.update_mu_iteration_criteria && + settings1.interval_update_mu == settings2.interval_update_mu && + settings1.polish == settings2.polish && + settings1.polish_refine_iter == settings2.polish_refine_iter && + settings1.delta == settings2.delta && + settings1.resume_admm == settings2.resume_admm && + settings1.try_high_accuracy == settings2.try_high_accuracy && settings1.preconditioner_max_iter == settings2.preconditioner_max_iter && settings1.preconditioner_accuracy == settings2.preconditioner_accuracy && settings1.eps_primal_inf == settings2.eps_primal_inf && diff --git a/include/proxsuite/proxqp/status.hpp b/include/proxsuite/proxqp/status.hpp index 55c5c8389..1ed870540 100644 --- a/include/proxsuite/proxqp/status.hpp +++ b/include/proxsuite/proxqp/status.hpp @@ -41,6 +41,14 @@ enum struct PreconditionerStatus IDENTITY // do not execute, hence use identity preconditioner (for init // method) }; +// POLISH OSQP STATUS +enum struct PolishStatus +{ + POLISH_SUCCEED, // better solution than without polishing. + POLISH_FAILED, // worst solution than without polishing. + POLISH_NO_ACTIVE_SET_FOUND, // no active set found to perform polishing. + POLISH_NOT_RUN // polishing has not been run yet. +}; } // namespace proxqp } // namespace proxsuite diff --git a/include/proxsuite/proxqp/utils/prints.hpp b/include/proxsuite/proxqp/utils/prints.hpp index 77984e531..6ccc3a8e9 100644 --- a/include/proxsuite/proxqp/utils/prints.hpp +++ b/include/proxsuite/proxqp/utils/prints.hpp @@ -19,13 +19,6 @@ print_line() std::cout << the_line << "\n" << std::endl; } -inline void -print_header() -{ - std::cout << "iter objective pri res dua res mu_in \n" - << std::endl; -} - inline void print_preambule() { diff --git a/include/proxsuite/solvers/common/utils.hpp b/include/proxsuite/solvers/common/utils.hpp new file mode 100644 index 000000000..f0e66d826 --- /dev/null +++ b/include/proxsuite/solvers/common/utils.hpp @@ -0,0 +1,1292 @@ +// +// Copyright (c) 2025 INRIA +// +/** + * @file utils.hpp + */ + +#ifndef PROXSUITE_SOLVERS_COMMON_UTILS_HPP +#define PROXSUITE_SOLVERS_COMMON_UTILS_HPP + +#include "proxsuite/proxqp/settings.hpp" +#include "proxsuite/proxqp/results.hpp" +#include "proxsuite/proxqp/dense/model.hpp" +#include "proxsuite/proxqp/dense/workspace.hpp" +#include "proxsuite/proxqp/dense/preconditioner/ruiz.hpp" +#include "proxsuite/proxqp/dense/helpers.hpp" +#include "proxsuite/proxqp/dense/linesearch.hpp" +#include "proxsuite/proxqp/dense/utils.hpp" +#include +#include +#include +#include +#include + +namespace proxsuite { +namespace common { + +namespace pp = proxsuite::proxqp; +namespace ppd = proxsuite::proxqp::dense; +namespace ppdp = proxsuite::proxqp::dense::preconditioner; +namespace plv = proxsuite::linalg::veg; + +/// +/// @brief This enum defines the different solvers implemented in ProxSuite. +/// +enum class QPSolver +{ + PROXQP, + OSQP +}; +/*! + * Prints the setup header. + * + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + * @param qpresults solver results. + * @param ruiz ruiz preconditioner. + * @param qp_solver PROXQP or OSQP. + */ +template +void +print_setup_header(const pp::Settings& qpsettings, + const pp::Results& qpresults, + const ppd::Model& qpmodel, + const bool box_constraints, + const pp::DenseBackend& dense_backend, + const pp::HessianType& hessian_type, + const common::QPSolver qp_solver) +{ + + switch (qp_solver) { + case common::QPSolver::PROXQP: + proxsuite::proxqp::print_preambule(); + break; + case common::QPSolver::OSQP: + proxsuite::osqp::print_preambule(); + break; + } + + // Print variables and constraints + std::cout << "problem: " << std::noshowpos << std::endl; + std::cout << " variables n = " << qpmodel.dim + << ", equality constraints n_eq = " << qpmodel.n_eq << ",\n" + << " inequality constraints n_in = " << qpmodel.n_in + << std::endl; + + // Print Settings + std::cout << "settings: " << std::endl; + std::cout << " backend = dense," << std::endl; + std::cout << " eps_abs = " << qpsettings.eps_abs + << " eps_rel = " << qpsettings.eps_rel << std::endl; + std::cout << " eps_prim_inf = " << qpsettings.eps_primal_inf + << ", eps_dual_inf = " << qpsettings.eps_dual_inf << "," + << std::endl; + + std::cout << " rho = " << qpresults.info.rho + << ", mu_eq = " << qpresults.info.mu_eq + << ", mu_in = " << qpresults.info.mu_in << "," << std::endl; + switch (qp_solver) { + case common::QPSolver::PROXQP: + std::cout << " max_iter = " << qpsettings.max_iter + << ", max_iter_in = " << qpsettings.max_iter_in << "," + << std::endl; + break; + case common::QPSolver::OSQP: + std::cout << " max_iter = " << qpsettings.max_iter << std::endl; + break; + } + if (box_constraints) { + std::cout << " box constraints: on, " << std::endl; + } else { + std::cout << " box constraints: off, " << std::endl; + } + switch (dense_backend) { + case pp::DenseBackend::PrimalDualLDLT: + std::cout << " dense backend: PrimalDualLDLT, " << std::endl; + break; + case pp::DenseBackend::PrimalLDLT: + std::cout << " dense backend: PrimalLDLT, " << std::endl; + break; + case pp::DenseBackend::Automatic: + break; + } + switch (hessian_type) { + case pp::HessianType::Dense: + std::cout << " problem type: Quadratic Program, " << std::endl; + break; + case pp::HessianType::Zero: + std::cout << " problem type: Linear Program, " << std::endl; + break; + case pp::HessianType::Diagonal: + std::cout + << " problem type: Quadratic Program with diagonal Hessian, " + << std::endl; + break; + } + if (qpsettings.compute_preconditioner) { + std::cout << " scaling: on, " << std::endl; + } else { + std::cout << " scaling: off, " << std::endl; + } + if (qpsettings.compute_timings) { + std::cout << " timings: on, " << std::endl; + } else { + std::cout << " timings: off, " << std::endl; + } + switch (qpsettings.initial_guess) { + case pp::InitialGuessStatus::WARM_START: + std::cout << " initial guess: warm start. \n" << std::endl; + break; + case pp::InitialGuessStatus::NO_INITIAL_GUESS: + std::cout << " initial guess: no initial guess. \n" << std::endl; + break; + case pp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT: + std::cout + << " initial guess: warm start with previous result. \n" + << std::endl; + break; + case pp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT: + std::cout + << " initial guess: cold start with previous result. \n" + << std::endl; + break; + case pp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS: + std::cout + << " initial guess: equality constrained initial guess. \n" + << std::endl; + } +} +/*! + * Prints the solver's statistics. + * + * @param qpsettings solver settings. + * @param qpresults solver results. + * @param qp_solver PROXQP or OSQP. + */ +template +void +print_solver_statistics(const pp::Settings& qpsettings, + const pp::Results& qpresults, + const common::QPSolver qp_solver) +{ + std::cout << "-------------------SOLVER STATISTICS-------------------" + << std::endl; + + switch (qp_solver) { + case common::QPSolver::PROXQP: { + std::cout << "outer iter: " << qpresults.info.iter_ext << std::endl; + std::cout << "total iter: " << qpresults.info.iter << std::endl; + std::cout << "mu updates: " << qpresults.info.mu_updates << std::endl; + std::cout << "rho updates: " << qpresults.info.rho_updates + << std::endl; + std::cout << "objective: " << qpresults.info.objValue << std::endl; + break; + } + case common::QPSolver::OSQP: { + std::cout << "admm iter: " << qpresults.info.iter_ext << std::endl; + std::cout << "polish calls: " << qpresults.info.polish_calls + << std::endl; + std::cout << "mu updates: " << qpresults.info.mu_updates << std::endl; + std::cout << "objective: " << qpresults.info.objValue << std::endl; + break; + } + } + + switch (qpresults.info.status) { + case pp::QPSolverOutput::PROXQP_SOLVED: { + std::cout << "status: " + << "Solved" << std::endl; + break; + } + case pp::QPSolverOutput::PROXQP_MAX_ITER_REACHED: { + std::cout << "status: " + << "Maximum number of iterations reached" << std::endl; + break; + } + case pp::QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE: { + std::cout << "status: " + << "Primal infeasible" << std::endl; + break; + } + case pp::QPSolverOutput::PROXQP_DUAL_INFEASIBLE: { + std::cout << "status: " + << "Dual infeasible" << std::endl; + break; + } + case pp::QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE: { + std::cout << "status: " + << "Solved closest primal feasible" << std::endl; + break; + } + case pp::QPSolverOutput::PROXQP_NOT_RUN: { + std::cout << "status: " + << "Solver not run" << std::endl; + break; + } + } + + if (qp_solver == QPSolver::OSQP) { + if (qpresults.info.polish_status == pp::PolishStatus::POLISH_NOT_RUN) { + std::cout << "polishing: " + << "Not run" << std::endl; + } else { + switch (qpresults.info.polish_status) { + case pp::PolishStatus::POLISH_SUCCEED: { + std::cout << "polishing: " + << "Succeed" << std::endl; + break; + } + case pp::PolishStatus::POLISH_FAILED: { + std::cout << "polishing: " + << "Failed" << std::endl; + break; + } + case pp::PolishStatus::POLISH_NO_ACTIVE_SET_FOUND: { + std::cout << "polishing: " + << "No active set found" << std::endl; + break; + } + case pp::PolishStatus::POLISH_NOT_RUN: { + break; + } + } + std::cout << "polishing options:" << std::endl; + std::cout << " resume_admm: " + << (qpsettings.resume_admm ? "ON" : "OFF") << std::endl; + std::cout << " high_accuracy: " + << (qpsettings.high_accuracy ? "ON" : "OFF") << std::endl; + if (!qpsettings.high_accuracy) { + std::cout << " try_high_accuracy: " + << (qpsettings.try_high_accuracy ? "ON" : "OFF") << std::endl; + } + std::cout << "polishing behaviour:" << std::endl; + std::cout << " resumed admm: " + << (qpresults.info.resumed_admm ? "Yes" : "No") << std::endl; + if (qpresults.info.resumed_admm) { + std::cout << " tried high accuracy: " + << (qpresults.info.tried_high_accuracy ? "Yes" : "No") + << std::endl; + } + } + } + + if (qpsettings.compute_timings) + std::cout << "run time [μs]: " << qpresults.info.solve_time << std::endl; + std::cout << "--------------------------------------------------------" + << std::endl; +} +/*! + * Prepares the next solve. Sets workspace to initialized and + * cleanups the information results. + * + * @param qpwork solver workspace. + * @param qpresults solver results. + */ +template +void +prepare_next_solve(pp::Results& qpresults, ppd::Workspace& qpwork) +{ + qpwork.dirty = true; + qpwork.is_initialized = true; // necessary because we call workspace cleanup + qpwork.is_first_solve = false; + + assert(!std::isnan(qpresults.info.pri_res)); + assert(!std::isnan(qpresults.info.dua_res)); + assert(!std::isnan(qpresults.info.duality_gap)); +} +/*! + * Setups and performs the factorization of the complete regularized KKT matrix + * of the problem (containing all of the equality and inequality constraints). + * It adds n_constraints (n_in + dim if box_constraints) rows/columns to the + * equality case KKT matrix. + * + * @param qpwork workspace of the solver. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpresults solution results. + */ +template +void +setup_factorization_complete_kkt(ppd::Workspace& qpwork, + const ppd::Model& qpmodel, + pp::Results& qpresults, + const pp::DenseBackend dense_backend, + const plv::isize n_constraints) +{ + proxsuite::linalg::veg::dynstack::DynStackMut stack{ + proxsuite::linalg::veg::from_slice_mut, qpwork.ldl_stack.as_mut() + }; + + T mu_in_neg(-qpresults.info.mu_in); + plv::isize n = qpmodel.dim; + plv::isize n_eq = qpmodel.n_eq; + LDLT_TEMP_MAT_UNINIT( + T, new_cols, n + n_eq + n_constraints, n_constraints, stack); + + for (plv::isize k = 0; k < n_constraints; ++k) { + auto col = new_cols.col(k); + if (k >= qpmodel.n_in) { + col.head(n).setZero(); + col[k - qpmodel.n_in] = qpwork.i_scaled[k - qpmodel.n_in]; + } else { + col.head(n) = (qpwork.C_scaled.row(k)); + } + col.tail(n_eq + n_constraints).setZero(); + col[n + n_eq + k] = mu_in_neg; + } + qpwork.ldl.insert_block_at(n + n_eq, new_cols, stack); + + qpwork.n_c = n_constraints; +} +/*! + * Setups the solver. + * In particular, it scales (Ruiz equilibration) the data, then + * builds the KKT matrix according to the algorihm, eg: + * proxqp: Builds the KKT with equality and activate inequality constraints + * osqp: Builds the KKT with all of the constraints. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + * @param qpresults solver results. + * @param ruiz ruiz preconditioner. + * @param qp_solver PROXQP or OSQP. + */ +template +void +setup_solver(const pp::Settings& qpsettings, + const ppd::Model& qpmodel, + pp::Results& qpresults, + ppd::Workspace& qpwork, + const bool box_constraints, + const pp::DenseBackend& dense_backend, + const pp::HessianType& hessian_type, + ppdp::RuizEquilibration& ruiz, + QPSolver qp_solver) +{ + plv::isize n_constraints(qpmodel.n_in); + if (box_constraints) { + n_constraints += qpmodel.dim; + } + if (qpsettings.compute_timings) { + qpwork.timer.stop(); + qpwork.timer.start(); + } + if (qpsettings.verbose) { + print_setup_header(qpsettings, + qpresults, + qpmodel, + box_constraints, + dense_backend, + hessian_type, + qp_solver); + } + if (qpwork.dirty) { // the following is used when a solve has already been + // executed (and without any intermediary model update) + switch (qpsettings.initial_guess) { + case pp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS: { + qpwork.cleanup(box_constraints); + qpresults.cleanup(qpsettings); + break; + } + case pp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT: { + // keep solutions but restart workspace and results + qpwork.cleanup(box_constraints); + qpresults.cold_start(qpsettings); + ruiz.scale_primal_in_place( + { proxsuite::proxqp::from_eigen, qpresults.x }); + ruiz.scale_dual_in_place_eq( + { proxsuite::proxqp::from_eigen, qpresults.y }); + ruiz.scale_dual_in_place_in( + { proxsuite::proxqp::from_eigen, qpresults.z.head(qpmodel.n_in) }); + if (box_constraints) { + ruiz.scale_box_dual_in_place_in( + { proxsuite::proxqp::from_eigen, qpresults.z.tail(qpmodel.dim) }); + } + break; + } + case pp::InitialGuessStatus::NO_INITIAL_GUESS: { + qpwork.cleanup(box_constraints); + qpresults.cleanup(qpsettings); + break; + } + case pp::InitialGuessStatus::WARM_START: { + qpwork.cleanup(box_constraints); + qpresults.cold_start( + qpsettings); // because there was already a solve, + // precond was already computed if set so + ruiz.scale_primal_in_place( + { proxsuite::proxqp::from_eigen, + qpresults + .x }); // it contains the value given in entry for warm start + ruiz.scale_dual_in_place_eq( + { proxsuite::proxqp::from_eigen, qpresults.y }); + ruiz.scale_dual_in_place_in( + { proxsuite::proxqp::from_eigen, qpresults.z.head(qpmodel.n_in) }); + if (box_constraints) { + ruiz.scale_box_dual_in_place_in( + { proxsuite::proxqp::from_eigen, qpresults.z.tail(qpmodel.dim) }); + } + break; + } + case pp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT: { + // keep workspace and results solutions except statistics + // std::cout << "i keep previous solution" << std::endl; + qpresults.cleanup_statistics(); + ruiz.scale_primal_in_place( + { proxsuite::proxqp::from_eigen, qpresults.x }); + ruiz.scale_dual_in_place_eq( + { proxsuite::proxqp::from_eigen, qpresults.y }); + ruiz.scale_dual_in_place_in( + { proxsuite::proxqp::from_eigen, qpresults.z.head(qpmodel.n_in) }); + if (box_constraints) { + ruiz.scale_box_dual_in_place_in( + { proxsuite::proxqp::from_eigen, qpresults.z.tail(qpmodel.dim) }); + } + break; + } + } + if (qpsettings.initial_guess != + pp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT) { + switch (hessian_type) { + case pp::HessianType::Zero: + break; + case pp::HessianType::Dense: + qpwork.H_scaled = qpmodel.H; + break; + case pp::HessianType::Diagonal: + qpwork.H_scaled = qpmodel.H; + break; + } + qpwork.g_scaled = qpmodel.g; + qpwork.A_scaled = qpmodel.A; + qpwork.b_scaled = qpmodel.b; + qpwork.C_scaled = qpmodel.C; + qpwork.u_scaled = qpmodel.u; + qpwork.l_scaled = qpmodel.l; + proxsuite::proxqp::dense::setup_equilibration( + qpwork, + qpsettings, + box_constraints, + hessian_type, + ruiz, + false); // reuse previous equilibration + proxsuite::proxqp::dense::setup_factorization( + qpwork, qpmodel, qpresults, dense_backend, hessian_type); + } + switch (qpsettings.initial_guess) { + case pp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS: { + compute_equality_constrained_initial_guess(qpwork, + qpsettings, + qpmodel, + n_constraints, + dense_backend, + hessian_type, + qpresults); + switch (qp_solver) { + case common::QPSolver::OSQP: { + setup_factorization_complete_kkt( + qpwork, qpmodel, qpresults, dense_backend, n_constraints); + } break; + } + break; + } + case pp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT: { + switch (qp_solver) { + case common::QPSolver::PROXQP: { + //!\ TODO in a quicker way + qpwork.n_c = 0; + for (plv::isize i = 0; i < n_constraints; i++) { + if (qpresults.z[i] != 0) { + qpwork.active_inequalities[i] = true; + } else { + qpwork.active_inequalities[i] = false; + } + } + ppd::linesearch::active_set_change( + qpmodel, qpresults, dense_backend, n_constraints, qpwork); + } break; + case common::QPSolver::OSQP: { + setup_factorization_complete_kkt( + qpwork, qpmodel, qpresults, dense_backend, n_constraints); + } break; + } + break; + } + case pp::InitialGuessStatus::NO_INITIAL_GUESS: { + switch (qp_solver) { + case common::QPSolver::OSQP: { + setup_factorization_complete_kkt( + qpwork, qpmodel, qpresults, dense_backend, n_constraints); + } break; + } + break; + } + case pp::InitialGuessStatus::WARM_START: { + switch (qp_solver) { + case common::QPSolver::PROXQP: { + //!\ TODO in a quicker way + qpwork.n_c = 0; + for (plv::isize i = 0; i < n_constraints; i++) { + if (qpresults.z[i] != 0) { + qpwork.active_inequalities[i] = true; + } else { + qpwork.active_inequalities[i] = false; + } + } + ppd::linesearch::active_set_change( + qpmodel, qpresults, dense_backend, n_constraints, qpwork); + } break; + case common::QPSolver::OSQP: { + setup_factorization_complete_kkt( + qpwork, qpmodel, qpresults, dense_backend, n_constraints); + } break; + } + break; + } + case pp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT: { + // keep workspace and results solutions except statistics + // std::cout << "i use previous solution" << std::endl; + // meaningful for when one wants to warm start with previous result with + // the same QP model + switch (qp_solver) { + case common::QPSolver::OSQP: { + setup_factorization_complete_kkt( + qpwork, qpmodel, qpresults, dense_backend, n_constraints); + } break; + } + break; + } + } + } else { // the following is used for a first solve after initializing or + // updating the Qp object + switch (qpsettings.initial_guess) { + case pp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS: { + switch (qp_solver) { + case common::QPSolver::PROXQP: { + proxsuite::proxqp::dense::setup_factorization( + qpwork, qpmodel, qpresults, dense_backend, hessian_type); + compute_equality_constrained_initial_guess(qpwork, + qpsettings, + qpmodel, + n_constraints, + dense_backend, + hessian_type, + qpresults); + } break; + case common::QPSolver::OSQP: { + if (qpwork.is_first_solve) { + qpwork.timer_factorization_complete_kkt.stop(); + qpwork.timer_factorization_complete_kkt.start(); + proxsuite::proxqp::dense::setup_factorization( + qpwork, qpmodel, qpresults, dense_backend, hessian_type); + qpwork.timer_factorization_complete_kkt.stop(); + qpwork.factorization_time_complete_kkt = + qpwork.timer_factorization_complete_kkt.elapsed().user; + // std::cout << "Time setup_factorization: " + // << + // qpwork.timer_factorization_complete_kkt.elapsed().user + // << std::endl; + compute_equality_constrained_initial_guess(qpwork, + qpsettings, + qpmodel, + n_constraints, + dense_backend, + hessian_type, + qpresults); + qpwork.timer_factorization_complete_kkt.start(); + setup_factorization_complete_kkt( + qpwork, qpmodel, qpresults, dense_backend, n_constraints); + qpwork.timer_factorization_complete_kkt.stop(); + qpwork.factorization_time_complete_kkt += + qpwork.timer_factorization_complete_kkt.elapsed().user; + // std::cout << "Time setup_factorization_compute_kkt: " + // << + // qpwork.timer_factorization_complete_kkt.elapsed().user + // << std::endl; + } else { // Keep the same first factorization time for mu update + proxsuite::proxqp::dense::setup_factorization( + qpwork, qpmodel, qpresults, dense_backend, hessian_type); + compute_equality_constrained_initial_guess(qpwork, + qpsettings, + qpmodel, + n_constraints, + dense_backend, + hessian_type, + qpresults); + setup_factorization_complete_kkt( + qpwork, qpmodel, qpresults, dense_backend, n_constraints); + } + } break; + } + break; + } + case pp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT: { + //!\ TODO in a quicker way + ruiz.scale_primal_in_place( + { proxsuite::proxqp::from_eigen, + qpresults + .x }); // meaningful for when there is an upate of the model and + // one wants to warm start with previous result + ruiz.scale_dual_in_place_eq( + { proxsuite::proxqp::from_eigen, qpresults.y }); + ruiz.scale_dual_in_place_in( + { proxsuite::proxqp::from_eigen, qpresults.z.head(qpmodel.n_in) }); + if (box_constraints) { + ruiz.scale_box_dual_in_place_in( + { proxsuite::proxqp::from_eigen, qpresults.z.tail(qpmodel.dim) }); + } + setup_factorization( + qpwork, qpmodel, qpresults, dense_backend, hessian_type); + switch (qp_solver) { + case common::QPSolver::PROXQP: { + //!\ TODO in a quicker way + qpwork.n_c = 0; + for (plv::isize i = 0; i < n_constraints; i++) { + if (qpresults.z[i] != 0) { + qpwork.active_inequalities[i] = true; + } else { + qpwork.active_inequalities[i] = false; + } + } + ppd::linesearch::active_set_change( + qpmodel, qpresults, dense_backend, n_constraints, qpwork); + } break; + case common::QPSolver::OSQP: { + setup_factorization_complete_kkt( + qpwork, qpmodel, qpresults, dense_backend, n_constraints); + } break; + } + break; + } + case pp::InitialGuessStatus::NO_INITIAL_GUESS: { + switch (qp_solver) { + case common::QPSolver::PROXQP: { + setup_factorization( + qpwork, qpmodel, qpresults, dense_backend, hessian_type); + } break; + case common::QPSolver::OSQP: { + if (qpwork.is_first_solve) { + qpwork.timer_factorization_complete_kkt.stop(); + qpwork.timer_factorization_complete_kkt.start(); + setup_factorization( + qpwork, qpmodel, qpresults, dense_backend, hessian_type); + setup_factorization_complete_kkt( + qpwork, qpmodel, qpresults, dense_backend, n_constraints); + qpwork.timer_factorization_complete_kkt.stop(); + qpwork.factorization_time_complete_kkt = + qpwork.timer_factorization_complete_kkt.elapsed().user; + } else { // Keep the same first factorization time for mu update + setup_factorization( + qpwork, qpmodel, qpresults, dense_backend, hessian_type); + setup_factorization_complete_kkt( + qpwork, qpmodel, qpresults, dense_backend, n_constraints); + } + } break; + } + break; + } + case pp::InitialGuessStatus::WARM_START: { + //!\ TODO in a quicker way + ruiz.scale_primal_in_place( + { proxsuite::proxqp::from_eigen, qpresults.x }); + ruiz.scale_dual_in_place_eq( + { proxsuite::proxqp::from_eigen, qpresults.y }); + ruiz.scale_dual_in_place_in( + { proxsuite::proxqp::from_eigen, qpresults.z.head(qpmodel.n_in) }); + if (box_constraints) { + ruiz.scale_box_dual_in_place_in( + { proxsuite::proxqp::from_eigen, qpresults.z.tail(qpmodel.dim) }); + } + switch (qp_solver) { + case common::QPSolver::PROXQP: { + setup_factorization( + qpwork, qpmodel, qpresults, dense_backend, hessian_type); + //!\ TODO in a quicker way + qpwork.n_c = 0; + for (plv::isize i = 0; i < n_constraints; i++) { + if (qpresults.z[i] != 0) { + qpwork.active_inequalities[i] = true; + } else { + qpwork.active_inequalities[i] = false; + } + } + ppd::linesearch::active_set_change( + qpmodel, qpresults, dense_backend, n_constraints, qpwork); + } break; + case common::QPSolver::OSQP: { + if (qpwork.is_first_solve) { + qpwork.timer_factorization_complete_kkt.stop(); + qpwork.timer_factorization_complete_kkt.start(); + setup_factorization( + qpwork, qpmodel, qpresults, dense_backend, hessian_type); + setup_factorization_complete_kkt( + qpwork, qpmodel, qpresults, dense_backend, n_constraints); + qpwork.timer_factorization_complete_kkt.stop(); + qpwork.factorization_time_complete_kkt = + qpwork.timer_factorization_complete_kkt.elapsed().user; + } else { // Keep the same first factorization time for mu update + setup_factorization( + qpwork, qpmodel, qpresults, dense_backend, hessian_type); + setup_factorization_complete_kkt( + qpwork, qpmodel, qpresults, dense_backend, n_constraints); + } + } break; + } + break; + } + case pp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT: { + // std::cout << "i refactorize from previous solution" << std::endl; + ruiz.scale_primal_in_place( + { proxsuite::proxqp::from_eigen, + qpresults + .x }); // meaningful for when there is an upate of the model and + // one wants to warm start with previous result + ruiz.scale_dual_in_place_eq( + { proxsuite::proxqp::from_eigen, qpresults.y }); + ruiz.scale_dual_in_place_in( + { proxsuite::proxqp::from_eigen, qpresults.z.head(qpmodel.n_in) }); + if (box_constraints) { + ruiz.scale_box_dual_in_place_in( + { proxsuite::proxqp::from_eigen, qpresults.z.tail(qpmodel.dim) }); + } + if (qpwork.refactorize) { // refactorization only when one of the + // matrices has changed or one proximal + // parameter has changed + setup_factorization( + qpwork, qpmodel, qpresults, dense_backend, hessian_type); + switch (qp_solver) { + case common::QPSolver::PROXQP: { + //!\ TODO in a quicker way + qpwork.n_c = 0; + for (plv::isize i = 0; i < n_constraints; i++) { + if (qpresults.z[i] != 0) { + qpwork.active_inequalities[i] = true; + } else { + qpwork.active_inequalities[i] = false; + } + } + ppd::linesearch::active_set_change( + qpmodel, qpresults, dense_backend, n_constraints, qpwork); + } break; + case common::QPSolver::OSQP: { + setup_factorization_complete_kkt( + qpwork, qpmodel, qpresults, dense_backend, n_constraints); + } break; + } + break; + } + } + } + } +} +/*! + * Computes the scaled primal residual for inequality. + * Used to define the sets of upper and lower inequality active constraints. + * + * @param qpsettings solver settings. + * @param qpmodel QP problem model. + * @param qpresults solver results. + * @param qpwork solver workspace. + */ +template +void +compute_scaled_primal_residual_ineq(const pp::Settings& qpsettings, + const ppd::Model& qpmodel, + pp::Results& qpresults, + ppd::Workspace& qpwork, + const bool box_constraints, + ppdp::RuizEquilibration& ruiz, + QPSolver qp_solver) +{ + switch (qp_solver) { + case QPSolver::PROXQP: { + ruiz.scale_primal_residual_in_place_in( + pp::VectorViewMut{ pp::from_eigen, + qpwork.primal_residual_in_scaled_up.head( + qpmodel.n_in) }); // contains now scaled(Cx) + if (box_constraints) { + ruiz.scale_box_primal_residual_in_place_in( + pp::VectorViewMut{ pp::from_eigen, + qpwork.primal_residual_in_scaled_up.tail( + qpmodel.dim) }); // contains now scaled(x) + } + qpwork.primal_residual_in_scaled_up += + qpwork.z_prev * + qpresults.info.mu_in; // contains now scaled(Cx+z_prev*mu_in) + switch (qpsettings.merit_function_type) { + case pp::MeritFunctionType::GPDAL: + qpwork.primal_residual_in_scaled_up += + (qpsettings.alpha_gpdal - 1.) * qpresults.info.mu_in * qpresults.z; + break; + case pp::MeritFunctionType::PDAL: + break; + } + qpresults.si = qpwork.primal_residual_in_scaled_up; + qpwork.primal_residual_in_scaled_up.head(qpmodel.n_in) -= + qpwork.u_scaled; // contains now scaled(Cx-u+z_prev*mu_in) + qpresults.si.head(qpmodel.n_in) -= + qpwork.l_scaled; // contains now scaled(Cx-l+z_prev*mu_in) + if (box_constraints) { + qpwork.primal_residual_in_scaled_up.tail(qpmodel.dim) -= + qpwork.u_box_scaled; // contains now scaled(Cx-u+z_prev*mu_in) + qpresults.si.tail(qpmodel.dim) -= + qpwork.l_box_scaled; // contains now scaled(Cx-l+z_prev*mu_in) + } + break; + } + case QPSolver::OSQP: { + qpwork.primal_residual_in_scaled_up = qpwork.zeta_in + qpwork.z_prev; + qpresults.si = qpwork.primal_residual_in_scaled_up; + qpwork.primal_residual_in_scaled_up.head(qpmodel.n_in) -= + qpwork.u_scaled; // contains now scaled(zeta_in-u+z_prev) + qpresults.si.head(qpmodel.n_in) -= + qpwork.l_scaled; // contains now scaled(zeta_in-l+z_prev) + if (box_constraints) { + qpwork.primal_residual_in_scaled_up.tail(qpmodel.dim) -= + qpwork.u_box_scaled; // contains now scaled(zeta-u+z_prev) + qpresults.si.tail(qpmodel.dim) -= + qpwork.l_box_scaled; // contains now scaled(zeta-l+z_prev) + } + break; + } + } +} +/*! + * Computes the scaled primal residual. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + * @param qpresults solver results. + */ +template +void +global_primal_residual_scaled(const ppd::Model& qpmodel, + pp::Results& qpresults, + ppd::Workspace& qpwork, + const bool box_constraints) +{ + qpresults.se.noalias() = qpwork.A_scaled * qpresults.x - qpwork.b_scaled; + + qpwork.primal_residual_in_scaled_up.head(qpmodel.n_in).noalias() = + qpwork.C_scaled * qpresults.x; + if (box_constraints) { + qpwork.primal_residual_in_scaled_up.tail(qpmodel.dim) = + qpwork.i_scaled.array() * qpresults.x.array(); + } + + qpresults.si.head(qpmodel.n_in) = + helpers::positive_part( + qpwork.primal_residual_in_scaled_up.head(qpmodel.n_in) - + qpwork.u_scaled) + + helpers::negative_part( + qpwork.primal_residual_in_scaled_up.head(qpmodel.n_in) - qpwork.l_scaled); + if (box_constraints) { + qpresults.si.tail(qpmodel.dim) = + helpers::positive_part( + qpwork.primal_residual_in_scaled_up.tail(qpmodel.dim) - + qpwork.u_box_scaled) + + helpers::negative_part( + qpwork.primal_residual_in_scaled_up.tail(qpmodel.dim) - + qpwork.l_box_scaled); + } + + qpwork.primal_residual_scaled.head(qpmodel.n_eq) = qpresults.se; + qpwork.primal_residual_scaled.segment(qpmodel.n_eq, qpmodel.n_in) = + qpresults.si.head(qpmodel.n_in); + if (box_constraints) { + qpwork.primal_residual_scaled.tail(qpmodel.dim) = + qpresults.si.tail(qpmodel.dim); + } +} +/*! + * Computes the scaled dual residual. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + * @param qpresults solver results. + */ +template +void +global_dual_residual_scaled(pp::Results& qpresults, + ppd::Workspace& qpwork, + const ppd::Model& qpmodel, + const bool box_constraints, + const pp::HessianType& hessian_type) +{ + qpwork.dual_residual_scaled = qpwork.g_scaled; + + switch (hessian_type) { + case pp::HessianType::Zero: + break; + case pp::HessianType::Dense: + qpwork.CTz.noalias() = + qpwork.H_scaled.template selfadjointView() * qpresults.x; + qpwork.dual_residual_scaled += qpwork.CTz; + break; + case pp::HessianType::Diagonal: + qpwork.CTz.array() = + qpwork.H_scaled.diagonal().array() * qpresults.x.array(); + qpwork.dual_residual_scaled += qpwork.CTz; + break; + } + + qpwork.CTz.noalias() = qpwork.A_scaled.transpose() * qpresults.y; + qpwork.dual_residual_scaled += qpwork.CTz; + + qpwork.CTz.noalias() = + qpwork.C_scaled.transpose() * qpresults.z.head(qpmodel.n_in); + qpwork.dual_residual_scaled += qpwork.CTz; + if (box_constraints) { + qpwork.CTz.noalias() = qpresults.z.tail(qpmodel.dim); + qpwork.CTz.array() *= qpwork.i_scaled.array(); + qpwork.dual_residual_scaled += qpwork.CTz; + } +} +/*! + * Computes the objective function. + * + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpresults solver results. + */ +template +void +compute_objective(const ppd::Model& qpmodel, pp::Results& qpresults) +{ + qpresults.info.objValue = 0; + for (Eigen::Index j = 0; j < qpmodel.dim; ++j) { + qpresults.info.objValue += + 0.5 * (qpresults.x(j) * qpresults.x(j)) * qpmodel.H(j, j); + qpresults.info.objValue += + qpresults.x(j) * T(qpmodel.H.col(j) + .tail(qpmodel.dim - j - 1) + .dot(qpresults.x.tail(qpmodel.dim - j - 1))); + } + qpresults.info.objValue += (qpmodel.g).dot(qpresults.x); +} +/*! + * Computes the objective function. + * + * @param qpresults solver results. + * @param qpwork solver workspace. + */ +template +void +compute_timings(pp::Results& qpresults, ppd::Workspace& qpwork) +{ + qpresults.info.solve_time = qpwork.timer.elapsed().user; // in microseconds + qpresults.info.run_time = + qpresults.info.solve_time + qpresults.info.setup_time; +} +/*! + * Unscales the solver once the algorithm is finished. + * + * @param qpsettings solver settings. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpresults solver results. + */ +template +void +unscale_solver(const pp::Settings& qpsettings, + const ppd::Model& qpmodel, + pp::Results& qpresults, + const bool box_constraints, + ppdp::RuizEquilibration& ruiz) +{ + ruiz.unscale_primal_in_place( + pp::VectorViewMut{ pp::from_eigen, qpresults.x }); + ruiz.unscale_dual_in_place_eq( + pp::VectorViewMut{ pp::from_eigen, qpresults.y }); + ruiz.unscale_dual_in_place_in( + pp::VectorViewMut{ pp::from_eigen, qpresults.z.head(qpmodel.n_in) }); + if (box_constraints) { + ruiz.unscale_box_dual_in_place_in( + pp::VectorViewMut{ pp::from_eigen, qpresults.z.tail(qpmodel.dim) }); + } + if (qpsettings.primal_infeasibility_solving && + qpresults.info.status == pp::QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { + ruiz.unscale_primal_residual_in_place_eq( + pp::VectorViewMut{ pp::from_eigen, qpresults.se }); + ruiz.unscale_primal_residual_in_place_in( + pp::VectorViewMut{ pp::from_eigen, qpresults.si.head(qpmodel.n_in) }); + if (box_constraints) { + ruiz.unscale_box_primal_residual_in_place_in( + pp::VectorViewMut{ pp::from_eigen, qpresults.si.tail(qpmodel.dim) }); + } + } +} +/*! + * Computes the residuals and the feasibility of the problem, then update it + * and stops the algorithm if needed. + * + * @param qpsettings solver settings. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpresults solver results. + * @param qpwork solver workspace. + * @param ruiz ruiz preconditioner. + * @param qp_solver PROXQP or OSQP. + */ +template +bool +is_solved( // + const pp::Settings& qpsettings, + const ppd::Model& qpmodel, + pp::Results& qpresults, + ppd::Workspace& qpwork, + const bool box_constraints, + const pp::HessianType& hessian_type, + ppdp::RuizEquilibration& ruiz, + QPSolver qp_solver, + T& primal_feasibility_eq_rhs_0, + T& primal_feasibility_in_rhs_0, + T& primal_feasibility_eq_lhs, + T& primal_feasibility_in_lhs, + T& primal_feasibility_lhs, + T& dual_feasibility_lhs, + T& dual_feasibility_rhs_0, + T& dual_feasibility_rhs_1, + T& dual_feasibility_rhs_3, + T& rhs_duality_gap, + T& duality_gap, + T& scaled_eps, + T& scaled_eps_rel, + plv::i64 iter) +{ + + ppd::global_primal_residual(qpmodel, + qpresults, + qpsettings, + qpwork, + ruiz, + box_constraints, + primal_feasibility_lhs, + primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0, + primal_feasibility_eq_lhs, + primal_feasibility_in_lhs); + + ppd::global_dual_residual(qpresults, + qpwork, + qpmodel, + box_constraints, + ruiz, + dual_feasibility_lhs, + dual_feasibility_rhs_0, + dual_feasibility_rhs_1, + dual_feasibility_rhs_3, + rhs_duality_gap, + duality_gap, + hessian_type); + + qpresults.info.pri_res = primal_feasibility_lhs; + qpresults.info.dua_res = dual_feasibility_lhs; + qpresults.info.duality_gap = duality_gap; + + T rhs_pri(scaled_eps); + if (scaled_eps_rel != 0) { + rhs_pri += scaled_eps_rel * std::max(primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0); + } + bool is_primal_feasible = primal_feasibility_lhs <= rhs_pri; + + T rhs_dua(scaled_eps); + if (scaled_eps_rel != 0) { + rhs_dua += + scaled_eps_rel * + std::max(std::max(dual_feasibility_rhs_3, dual_feasibility_rhs_0), + std::max(dual_feasibility_rhs_1, qpwork.dual_feasibility_rhs_2)); + } + bool is_dual_feasible = dual_feasibility_lhs <= rhs_dua; + + if (qpsettings.verbose) { + ruiz.unscale_primal_in_place( + pp::VectorViewMut{ pp::from_eigen, qpresults.x }); + ruiz.unscale_dual_in_place_eq( + pp::VectorViewMut{ pp::from_eigen, qpresults.y }); + ruiz.unscale_dual_in_place_in( + pp::VectorViewMut{ pp::from_eigen, qpresults.z.head(qpmodel.n_in) }); + if (box_constraints) { + ruiz.unscale_box_dual_in_place_in( + pp::VectorViewMut{ pp::from_eigen, qpresults.z.tail(qpmodel.dim) }); + } + + compute_objective(qpmodel, qpresults); + + switch (qp_solver) { + case common::QPSolver::PROXQP: { + std::cout << "\033[1;32m[outer iteration " << iter + 1 << "]\033[0m" + << std::endl; + std::cout << std::scientific << std::setw(2) << std::setprecision(2) + << " | primal residual=" << qpresults.info.pri_res + << " | dual residual=" << qpresults.info.dua_res + << " | duality gap=" << qpresults.info.duality_gap + << " | mu_in=" << qpresults.info.mu_in + << " | rho=" << qpresults.info.rho << std::endl; + break; + case common::QPSolver::OSQP: { + std::cout << "\033[1;32m[admm iteration " << iter + 1 << "]\033[0m" + << std::endl; + std::cout << std::scientific << std::setw(2) << std::setprecision(2) + << " | primal residual=" << qpresults.info.pri_res + << " | dual residual=" << qpresults.info.dua_res + << " | duality gap=" << qpresults.info.duality_gap + << " | rho=" << qpresults.info.rho + << " | mu_in=" << qpresults.info.mu_in + << " | mu_eq=" << qpresults.info.mu_eq << std::endl; + break; + } + } + } + + ruiz.scale_primal_in_place( + pp::VectorViewMut{ pp::from_eigen, qpresults.x }); + ruiz.scale_dual_in_place_eq( + pp::VectorViewMut{ pp::from_eigen, qpresults.y }); + ruiz.scale_dual_in_place_in( + pp::VectorViewMut{ pp::from_eigen, qpresults.z.head(qpmodel.n_in) }); + if (box_constraints) { + ruiz.scale_box_dual_in_place_in( + pp::VectorViewMut{ pp::from_eigen, qpresults.z.tail(qpmodel.dim) }); + } + } + + if (is_primal_feasible && is_dual_feasible) { + if (qpsettings.check_duality_gap) { + if (std::fabs(qpresults.info.duality_gap) <= + qpsettings.eps_duality_gap_abs + + qpsettings.eps_duality_gap_rel * rhs_duality_gap) { + if (qpsettings.primal_infeasibility_solving && + qpresults.info.status == + pp::QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { + qpresults.info.status = + pp::QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE; + } else { + qpresults.info.status = pp::QPSolverOutput::PROXQP_SOLVED; + } + return true; + } + } else { + qpresults.info.status = pp::QPSolverOutput::PROXQP_SOLVED; + return true; + } + } + return false; +} +/*! + * Computes residuals, the infeasibility and updates the solver's status. + * + * @param qpwork solver workspace. + * @param qpmodel QP problem model as defined by the user (without any scaling + * performed). + * @param qpsettings solver settings. + * @param qpresults solver results. + * @param ruiz ruiz preconditioner. + */ +template +void +update_solver_status( // + const pp::Settings& qpsettings, + const ppd::Model& qpmodel, + pp::Results& qpresults, + ppd::Workspace& qpwork, + const bool box_constraints, + const pp::HessianType& hessian_type, + ppdp::RuizEquilibration& ruiz, + T& primal_feasibility_eq_rhs_0, + T& primal_feasibility_in_rhs_0, + T& primal_feasibility_eq_lhs, + T& primal_feasibility_in_lhs, + T& primal_feasibility_lhs_new, + T& dual_feasibility_lhs_new, + T& dual_feasibility_rhs_0, + T& dual_feasibility_rhs_1, + T& dual_feasibility_rhs_3, + T& rhs_duality_gap, + T& duality_gap, + T& scaled_eps, + T& scaled_eps_rel) +{ + ppd::global_primal_residual(qpmodel, + qpresults, + qpsettings, + qpwork, + ruiz, + box_constraints, + primal_feasibility_lhs_new, + primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0, + primal_feasibility_eq_lhs, + primal_feasibility_in_lhs); + + bool is_primal_feasible = + primal_feasibility_lhs_new <= + (scaled_eps + scaled_eps_rel * std::max(primal_feasibility_eq_rhs_0, + primal_feasibility_in_rhs_0)); + qpresults.info.pri_res = primal_feasibility_lhs_new; + + if (is_primal_feasible) { + ppd::global_dual_residual(qpresults, + qpwork, + qpmodel, + box_constraints, + ruiz, + dual_feasibility_lhs_new, + dual_feasibility_rhs_0, + dual_feasibility_rhs_1, + dual_feasibility_rhs_3, + rhs_duality_gap, + duality_gap, + hessian_type); + qpresults.info.dua_res = dual_feasibility_lhs_new; + qpresults.info.duality_gap = duality_gap; + + bool is_dual_feasible = + dual_feasibility_lhs_new <= + (scaled_eps + + scaled_eps_rel * + std::max( + std::max(dual_feasibility_rhs_3, dual_feasibility_rhs_0), + std::max(dual_feasibility_rhs_1, qpwork.dual_feasibility_rhs_2))); + + if (is_dual_feasible) { + if (qpsettings.check_duality_gap) { + if (std::fabs(qpresults.info.duality_gap) <= + qpsettings.eps_duality_gap_abs + + qpsettings.eps_duality_gap_rel * rhs_duality_gap) { + if (qpsettings.primal_infeasibility_solving && + qpresults.info.status == + pp::QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { + qpresults.info.status = + pp::QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE; + } else { + qpresults.info.status = pp::QPSolverOutput::PROXQP_SOLVED; + } + } + } else { + if (qpsettings.primal_infeasibility_solving && + qpresults.info.status == + pp::QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { + qpresults.info.status = + pp::QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE; + } else { + qpresults.info.status = pp::QPSolverOutput::PROXQP_SOLVED; + } + } + } + } +} + +} // namespace common +} // namespace proxsuite + +#endif /* end of include guard PROXSUITE_SOLVERS_COMMON_UTILS_HPP */ \ No newline at end of file diff --git a/include/proxsuite/solvers/common/wrapper.hpp b/include/proxsuite/solvers/common/wrapper.hpp new file mode 100644 index 000000000..6e74c55e5 --- /dev/null +++ b/include/proxsuite/solvers/common/wrapper.hpp @@ -0,0 +1,274 @@ +// +// Copyright (c) 2025 INRIA +// +/** + * @file utils.hpp + */ + +#ifndef PROXSUITE_SOLVERS_COMMON_WRAPPER_HPP +#define PROXSUITE_SOLVERS_COMMON_WRAPPER_HPP + +#include "proxsuite/proxqp/results.hpp" +#include "proxsuite/proxqp/dense/fwd.hpp" +#include "proxsuite/linalg/veg/internal/typedefs.hpp" + +namespace proxsuite { +namespace common { + +namespace ppd = proxsuite::proxqp::dense; +namespace plv = proxsuite::linalg::veg; + +/*! + * Generic function to solve the QP. Used in the functions solve() to solve the + * the problem without defining the API. There are no box constraints in the + * model. + * @param Qp QP object on which the problem is solved. + * @param H quadratic cost input defining the QP model. + * @param g linear cost input defining the QP model. + * @param A equality constraint matrix input defining the QP model. + * @param b equality constraint vector input defining the QP model. + * @param C inequality constraint matrix input defining the QP model. + * @param l lower inequality constraint vector input defining the QP model. + * @param u upper inequality constraint vector input defining the QP model. + * @param x primal warm start. + * @param y dual equality constraint warm start. + * @param z dual inequality constraint warm start. + * @param verbose if set to true, the solver prints more information about each + * iteration. + * @param compute_preconditioner bool parameter for executing or not the + * preconditioner. + * @param compute_timings boolean parameter for computing the solver timings. + * @param rho proximal step size wrt primal variable. + * @param mu_eq proximal step size wrt equality constrained multiplier. + * @param mu_in proximal step size wrt inequality constrained multiplier. + * @param eps_abs absolute accuracy threshold. + * @param eps_rel relative accuracy threshold. + * @param max_iter maximum number of iteration. + * @param initial_guess initial guess option for warm starting or not the + * initial iterate values. + * @param check_duality_gap If set to true, include the duality gap in absolute + * and relative stopping criteria. + * @param eps_duality_gap_abs absolute accuracy threshold for the duality-gap + * criterion. + * @param eps_duality_gap_rel relative accuracy threshold for the duality-gap + * criterion. + */ +template +proxqp::Results +solve_without_api(QPStruct& Qp, + optional> H, + optional> g, + optional> A, + optional> b, + optional> C, + optional> l, + optional> u, + optional> x, + optional> y, + optional> z, + optional eps_abs, + optional eps_rel, + optional rho, + optional mu_eq, + optional mu_in, + optional verbose, + bool compute_preconditioner, + bool compute_timings, + optional max_iter, + proxsuite::proxqp::InitialGuessStatus initial_guess, + bool check_duality_gap, + optional eps_duality_gap_abs, + optional eps_duality_gap_rel, + bool primal_infeasibility_solving, + optional manual_minimal_H_eigenvalue) +{ + Qp.settings.initial_guess = initial_guess; + Qp.settings.check_duality_gap = check_duality_gap; + + if (eps_abs != nullopt) { + Qp.settings.eps_abs = eps_abs.value(); + } + if (eps_rel != nullopt) { + Qp.settings.eps_rel = eps_rel.value(); + } + if (verbose != nullopt) { + Qp.settings.verbose = verbose.value(); + } + if (max_iter != nullopt) { + Qp.settings.max_iter = max_iter.value(); + } + if (eps_duality_gap_abs != nullopt) { + Qp.settings.eps_duality_gap_abs = eps_duality_gap_abs.value(); + } + if (eps_duality_gap_rel != nullopt) { + Qp.settings.eps_duality_gap_rel = eps_duality_gap_rel.value(); + } + Qp.settings.compute_timings = compute_timings; + Qp.settings.primal_infeasibility_solving = primal_infeasibility_solving; + if (manual_minimal_H_eigenvalue != nullopt) { + Qp.init(H, + g, + A, + b, + C, + l, + u, + compute_preconditioner, + rho, + mu_eq, + mu_in, + manual_minimal_H_eigenvalue.value()); + } else { + Qp.init( + H, g, A, b, C, l, u, compute_preconditioner, rho, mu_eq, mu_in, nullopt); + } + Qp.solve(x, y, z); + + return Qp.results; +} +/*! + * Generic function to solve the QP. Used in the functions solve() to solve the + * the problem without defining the API. There are box constraints in the model. + * @param Qp QP object on which the problem is solved. + * @param H quadratic cost input defining the QP model. + * @param g linear cost input defining the QP model. + * @param A equality constraint matrix input defining the QP model. + * @param b equality constraint vector input defining the QP model. + * @param C inequality constraint matrix input defining the QP model. + * @param l lower inequality constraint vector input defining the QP model. + * @param u upper inequality constraint vector input defining the QP model. + * @param l_box lower box inequality constraint vector input defining the QP + * model. + * @param u_box upper box inequality constraint vector input defining the QP + * model. + * @param x primal warm start. + * @param y dual equality constraint warm start. + * @param z dual inequality constraint warm start. + * @param verbose if set to true, the solver prints more information about each + * iteration. + * @param compute_preconditioner bool parameter for executing or not the + * preconditioner. + * @param compute_timings boolean parameter for computing the solver timings. + * @param rho proximal step size wrt primal variable. + * @param mu_eq proximal step size wrt equality constrained multiplier. + * @param mu_in proximal step size wrt inequality constrained multiplier. + * @param eps_abs absolute accuracy threshold. + * @param eps_rel relative accuracy threshold. + * @param max_iter maximum number of iteration. + * @param initial_guess initial guess option for warm starting or not the + * initial iterate values. + * @param check_duality_gap If set to true, include the duality gap in absolute + * and relative stopping criteria. + * @param eps_duality_gap_abs absolute accuracy threshold for the duality-gap + * criterion. + * @param eps_duality_gap_rel relative accuracy threshold for the duality-gap + * criterion. + */ +template +proxqp::Results +solve_without_api(QPStruct& Qp, + optional> H, + optional> g, + optional> A, + optional> b, + optional> C, + optional> l, + optional> u, + optional> l_box, + optional> u_box, + optional> x, + optional> y, + optional> z, + optional eps_abs, + optional eps_rel, + optional rho, + optional mu_eq, + optional mu_in, + optional verbose, + bool compute_preconditioner, + bool compute_timings, + optional max_iter, + proxsuite::proxqp::InitialGuessStatus initial_guess, + bool check_duality_gap, + optional eps_duality_gap_abs, + optional eps_duality_gap_rel, + bool primal_infeasibility_solving, + optional manual_minimal_H_eigenvalue) +{ + Qp.settings.initial_guess = initial_guess; + Qp.settings.check_duality_gap = check_duality_gap; + + if (eps_abs != nullopt) { + Qp.settings.eps_abs = eps_abs.value(); + } + if (eps_rel != nullopt) { + Qp.settings.eps_rel = eps_rel.value(); + } + if (verbose != nullopt) { + Qp.settings.verbose = verbose.value(); + } + if (max_iter != nullopt) { + Qp.settings.max_iter = max_iter.value(); + } + if (eps_duality_gap_abs != nullopt) { + Qp.settings.eps_duality_gap_abs = eps_duality_gap_abs.value(); + } + if (eps_duality_gap_rel != nullopt) { + Qp.settings.eps_duality_gap_rel = eps_duality_gap_rel.value(); + } + Qp.settings.compute_timings = compute_timings; + Qp.settings.primal_infeasibility_solving = primal_infeasibility_solving; + if (manual_minimal_H_eigenvalue != nullopt) { + Qp.init(H, + g, + A, + b, + C, + l, + u, + l_box, + u_box, + compute_preconditioner, + rho, + mu_eq, + mu_in, + manual_minimal_H_eigenvalue.value()); + } else { + Qp.init(H, + g, + A, + b, + C, + l, + u, + l_box, + u_box, + compute_preconditioner, + rho, + mu_eq, + mu_in, + nullopt); + } + Qp.solve(x, y, z); + + return Qp.results; +} +/*! + * Generic function to test wether two QP objects are equal. + * @param qp1 First QP object. + * @param qp2 Second QP object. + */ +template +bool +is_equal(const QPStruct& qp1, const QPStruct& qp2) +{ + bool value = qp1.model == qp2.model && qp1.settings == qp2.settings && + qp1.results == qp2.results && + qp1.is_box_constrained() == qp2.is_box_constrained(); + return value; +} + +} // namespace common +} // namespace proxsuite + +#endif /* end of include guard PROXSUITE_SOLVERS_COMMON_WRAPPER_HPP */ \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index dbea3694d..e5a5e2694 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -39,6 +39,13 @@ macro(proxsuite_test name path) add_dependencies(build_tests ${target_name}) endmacro() +proxsuite_test(osqp_dense_ruiz_equilibration src/osqp_dense_ruiz_equilibration.cpp) +proxsuite_test(osqp_dense_qp_eq src/osqp_dense_qp_eq.cpp) +proxsuite_test(osqp_dense_qp_unconstrained src/osqp_dense_unconstrained_qp.cpp) +proxsuite_test(osqp_dense_qp_solve src/osqp_dense_qp_solve.cpp) +proxsuite_test(osqp_dense_qp_with_eq_and_in src/osqp_dense_qp_with_eq_and_in.cpp) +proxsuite_test(osqp_cvxpy src/osqp_cvxpy.cpp) +proxsuite_test(osqp_dense_qp_wrapper src/osqp_dense_qp_wrapper.cpp) proxsuite_test(dense_ruiz_equilibration src/dense_ruiz_equilibration.cpp) proxsuite_test(dense_qp_eq src/dense_qp_eq.cpp) proxsuite_test(dense_qp_with_eq_and_in src/dense_qp_with_eq_and_in.cpp) @@ -87,6 +94,7 @@ endif() if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug" AND NOT MSVC) proxsuite_test(dense_maros_meszaros src/dense_maros_meszaros.cpp) + proxsuite_test(osqp_dense_maros_meszaros src/osqp_dense_maros_meszaros.cpp) proxsuite_test(sparse_maros_meszaros src/sparse_maros_meszaros.cpp) endif() diff --git a/test/src/dense_qp_eq.cpp b/test/src/dense_qp_eq.cpp index 2a6aa2541..d79cc02c2 100644 --- a/test/src/dense_qp_eq.cpp +++ b/test/src/dense_qp_eq.cpp @@ -253,4 +253,37 @@ DOCTEST_TEST_CASE("infeasible qp") DOCTEST_CHECK(qp.results.info.status == proxsuite::proxqp::QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE); +} + +DOCTEST_TEST_CASE("dual infeasible qp equality constraints") +{ + Eigen::Matrix H; + H << 0.0, 0.0, 0.0, 0.0; + H = 2 * H; + + Eigen::Matrix g; + g << 1.0, -1.0; + + Eigen::Matrix A; + A << 1, 1, 1, 1; + + Eigen::Matrix b; + b << 1, 1; + + int n = H.rows(); + int n_in = 0; + int n_eq = A.rows(); + + bool box_constraints = false; + proxqp::dense::QP qp{ + n, n_eq, n_in, box_constraints, proxqp::DenseBackend::PrimalDualLDLT + }; // creating QP object + qp.init(H, g, A, b, nullopt, nullopt, nullopt); + qp.settings.eps_rel = 0.; + qp.settings.eps_abs = 1e-9; + + qp.solve(); + + DOCTEST_CHECK(qp.results.info.status == + proxsuite::proxqp::QPSolverOutput::PROXQP_DUAL_INFEASIBLE); } \ No newline at end of file diff --git a/test/src/osqp_cvxpy.cpp b/test/src/osqp_cvxpy.cpp new file mode 100644 index 000000000..ac353e93f --- /dev/null +++ b/test/src/osqp_cvxpy.cpp @@ -0,0 +1,216 @@ +// +// Copyright (c) 2022 INRIA +// +#include +#include +#include +#include + +using T = double; +using namespace proxsuite; + +namespace pp = proxsuite::proxqp; +namespace ppd = proxsuite::proxqp::dense; +namespace pod = proxsuite::osqp::dense; + +template +using Mat = + Eigen::Matrix; +template +using Vec = Eigen::Matrix; + +// 1 +DOCTEST_TEST_CASE("3 dim test case from cvxpy, check feasibility") +{ + + std::cout << "---3 dim test case from cvxpy, check feasibility " << std::endl; + T eps_abs = T(1e-9); + ppd::isize dim = 3; + + Mat H = Mat(dim, dim); + H << 13.0, 12.0, -2.0, 12.0, 17.0, 6.0, -2.0, 6.0, 12.0; + + Vec g = Vec(dim); + g << -22.0, -14.5, 13.0; + + Mat C = Mat(dim, dim); + C << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; + + Vec l = Vec(dim); + l << -1.0, -1.0, -1.0; + + Vec u = Vec(dim); + u << 1.0, 1.0, 1.0; + // pp::Results results = pod::solve( + // H, g, nullopt, nullopt, C, l, u, nullopt, nullopt, nullopt, eps_abs, 0); + pp::Results results = pod::solve(H, + g, + nullopt, + nullopt, + C, + l, + u, + nullopt, + nullopt, + nullopt, + eps_abs, + 0, + T(1.E-6), + T(1.E-2), + T(1.E1)); + + T pri_res = (helpers::positive_part(C * results.x - u) + + helpers::negative_part(C * results.x - l)) + .lpNorm(); + T dua_res = + (H * results.x + g + C.transpose() * results.z).lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration (admm): " << results.info.iter_ext + << std::endl; + std::cout << "setup timing " << results.info.setup_time << " solve time " + << results.info.solve_time << std::endl; +} + +// 2 +DOCTEST_TEST_CASE("simple test case from cvxpy, check feasibility") +{ + + std::cout << "---simple test case from cvxpy, check feasibility " + << std::endl; + T eps_abs = T(1e-8); + // Solution polishing + // In that case, the polishing algorithm does not find any upper or lower + // constraint. Then the algorithm as described in the solver's paper would + // stop at a low precision (default 1e-3) and the test would fail. Original + // feature in this implementation (C): Introduction of the option resume_admm + // that consists in return to the ADMM iterations in case of polishing failing + // or no active sets founds. In that case, the QPs are directly solved, if + // possible, without the polishing. Note: Option set to true by default, as it + // allows the solver to pass the tests and problems in benchmarks, while keep + // providing relevant information (from polish status). + ppd::isize dim = 1; + + Mat H = Mat(dim, dim); + H << 20.0; + + Vec g = Vec(dim); + g << -10.0; + + Mat C = Mat(dim, dim); + C << 1.0; + + Vec l = Vec(dim); + l << 0.0; + + Vec u = Vec(dim); + u << 1.0; + // pp::Results results = pod::solve( + // H, g, nullopt, nullopt, C, l, u, nullopt, nullopt, nullopt, eps_abs, 0); + pp::Results results = pod::solve(H, + g, + nullopt, + nullopt, + C, + l, + u, + nullopt, + nullopt, + nullopt, + eps_abs, + 0, + T(1.E-6), + T(1.E-2), + T(1.E1)); + + T pri_res = (helpers::positive_part(C * results.x - u) + + helpers::negative_part(C * results.x - l)) + .lpNorm(); + T dua_res = + (H * results.x + g + C.transpose() * results.z).lpNorm(); + T x_sol = 0.5; + + DOCTEST_CHECK((x_sol - results.x.coeff(0, 0)) <= eps_abs); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration (admm): " << results.info.iter_ext + << std::endl; + std::cout << "setup timing " << results.info.setup_time << " solve time " + << results.info.solve_time << std::endl; +} + +// 3 +DOCTEST_TEST_CASE("simple test case from cvxpy, init with solution, check that " + "solver stays there") +{ + + std::cout << "---simple test case from cvxpy, init with solution, check that " + "solver stays there" + << std::endl; + T eps_abs = T(1e-4); + ppd::isize dim = 1; + + Mat H = Mat(dim, dim); + H << 20.0; + + Vec g = Vec(dim); + g << -10.0; + + Mat C = Mat(dim, dim); + C << 1.0; + + Vec l = Vec(dim); + l << 0.0; + + Vec u = Vec(dim); + u << 1.0; + + T x_sol = 0.5; + + proxqp::isize n_in(1); + proxqp::isize n_eq(0); + pod::QP qp{ dim, n_eq, n_in }; + qp.settings.eps_abs = eps_abs; + + qp.settings.default_mu_eq = T(1.E-2); + qp.settings.default_mu_in = T(1.E1); + + qp.init(H, g, nullopt, nullopt, C, u, l); + + ppd::Vec x = ppd::Vec(dim); + ppd::Vec z = ppd::Vec(n_in); + x << 0.5; + z << 0.0; + qp.solve(x, nullopt, z); + + T pri_res = (helpers::positive_part(C * qp.results.x - u) + + helpers::negative_part(C * qp.results.x - l)) + .lpNorm(); + T dua_res = (H * qp.results.x + g + C.transpose() * qp.results.z) + .lpNorm(); + + DOCTEST_CHECK(qp.results.info.iter_ext <= 0); + DOCTEST_CHECK((x_sol - qp.results.x.coeff(0, 0)) <= eps_abs); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "number of iterations (admm): " << qp.results.info.iter_ext + << std::endl; + if (qp.settings.polish) { + std::cout << "number of iterations (polishing): " + << qp.settings.polish_refine_iter << std::endl; + } + std::cout << "setup timing " << qp.results.info.setup_time << " solve time" + << qp.results.info.solve_time << std::endl; +} diff --git a/test/src/osqp_dense_maros_meszaros.cpp b/test/src/osqp_dense_maros_meszaros.cpp new file mode 100644 index 000000000..87e4a4393 --- /dev/null +++ b/test/src/osqp_dense_maros_meszaros.cpp @@ -0,0 +1,331 @@ +// +// Copyright (c) 2022 INRIA +// +#include +#include +#include +#include + +using namespace proxsuite; +namespace pp = proxsuite::proxqp; +namespace pod = proxsuite::osqp::dense; + +#define MAROS_MESZAROS_DIR PROBLEM_PATH "/data/maros_meszaros_data/" + +char const* files[] = { + // MAROS_MESZAROS_DIR "AUG2D.mat", // skipping + // MAROS_MESZAROS_DIR "AUG2DC.mat", // skipping + // MAROS_MESZAROS_DIR "AUG2DCQP.mat", // skipping + // MAROS_MESZAROS_DIR "AUG2DQP.mat", // skipping + // MAROS_MESZAROS_DIR "AUG3D.mat", // skipping + // MAROS_MESZAROS_DIR "AUG3DC.mat", // skipping + // MAROS_MESZAROS_DIR "AUG3DCQP.mat", // skipping + // MAROS_MESZAROS_DIR "AUG3DQP.mat", // skipping + // MAROS_MESZAROS_DIR "BOYD1.mat", // skipping + // MAROS_MESZAROS_DIR "BOYD2.mat", // skipping + // MAROS_MESZAROS_DIR "CONT-050.mat", // skipping + // MAROS_MESZAROS_DIR "CONT-100.mat", // skipping + // MAROS_MESZAROS_DIR "CONT-101.mat", // skipping + // MAROS_MESZAROS_DIR "CONT-200.mat", // skipping + // MAROS_MESZAROS_DIR "CONT-201.mat", // skipping + // MAROS_MESZAROS_DIR "CONT-300.mat", // skipping + // MAROS_MESZAROS_DIR "CVXQP1_L.mat", // skipping + // MAROS_MESZAROS_DIR "CVXQP1_M.mat", // skipping + // MAROS_MESZAROS_DIR "CVXQP1_S.mat", // Pass + // MAROS_MESZAROS_DIR "CVXQP2_L.mat", // skipping + // MAROS_MESZAROS_DIR "CVXQP2_M.mat", // skipping + // MAROS_MESZAROS_DIR "CVXQP2_S.mat", // Pass + // MAROS_MESZAROS_DIR "CVXQP3_L.mat", // skipping + // MAROS_MESZAROS_DIR "CVXQP3_M.mat", // skipping + // MAROS_MESZAROS_DIR "CVXQP3_S.mat", // Pass + // MAROS_MESZAROS_DIR "DPKLO1.mat", // Pass + // MAROS_MESZAROS_DIR "DTOC3.mat", // Skipping + // MAROS_MESZAROS_DIR "DUAL1.mat", // Pass + // MAROS_MESZAROS_DIR "DUAL2.mat", // Pass + // MAROS_MESZAROS_DIR "DUAL3.mat", // Pass + // MAROS_MESZAROS_DIR "DUAL4.mat", // Pass + // MAROS_MESZAROS_DIR "DUALC1.mat", // Pass + // MAROS_MESZAROS_DIR "DUALC2.mat", // Pass + // MAROS_MESZAROS_DIR "DUALC5.mat", // Pass + // MAROS_MESZAROS_DIR "DUALC8.mat", // Pass + // MAROS_MESZAROS_DIR "EXDATA.mat", // Skipping + // MAROS_MESZAROS_DIR "GENHS28.mat", // Pass + // MAROS_MESZAROS_DIR "GOULDQP2.mat", // Skipping + // MAROS_MESZAROS_DIR "GOULDQP3.mat", // Skipping + // MAROS_MESZAROS_DIR "HS118.mat", // Pass + // MAROS_MESZAROS_DIR "HS21.mat", // Pass + // MAROS_MESZAROS_DIR "HS268.mat", // Pass + // MAROS_MESZAROS_DIR "HS35.mat", // Pass + // MAROS_MESZAROS_DIR "HS35MOD.mat", // Pass + // MAROS_MESZAROS_DIR "HS51.mat", // Pass + // MAROS_MESZAROS_DIR "HS52.mat", // Pass + // MAROS_MESZAROS_DIR "HS53.mat", // Pass + // MAROS_MESZAROS_DIR "HS76.mat", // Pass + // MAROS_MESZAROS_DIR "HUES-MOD.mat", // Skipping + // MAROS_MESZAROS_DIR "HUESTIS.mat", // Skipping + // MAROS_MESZAROS_DIR "KSIP.mat", // Skipping + // MAROS_MESZAROS_DIR "LASER.mat", // Skipping + // MAROS_MESZAROS_DIR "LISWET1.mat", // Skipping + // MAROS_MESZAROS_DIR "LISWET10.mat", // Skipping + // MAROS_MESZAROS_DIR "LISWET11.mat", // Skipping + // MAROS_MESZAROS_DIR "LISWET12.mat", // Skipping + // MAROS_MESZAROS_DIR "LISWET2.mat", // Skipping + // MAROS_MESZAROS_DIR "LISWET3.mat", // Skipping + // MAROS_MESZAROS_DIR "LISWET4.mat", // Skipping + // MAROS_MESZAROS_DIR "LISWET5.mat", // Skipping + // MAROS_MESZAROS_DIR "LISWET6.mat", // Skipping + // MAROS_MESZAROS_DIR "LISWET7.mat", // Skipping + // MAROS_MESZAROS_DIR "LISWET8.mat", // Skipping + // MAROS_MESZAROS_DIR "LISWET9.mat", // Skipping + // MAROS_MESZAROS_DIR "LOTSCHD.mat", // Pass + // MAROS_MESZAROS_DIR "MOSARQP1.mat", // Skipping + // MAROS_MESZAROS_DIR "MOSARQP2.mat", // Skipping + // MAROS_MESZAROS_DIR "POWELL20.mat", // Skipping + // MAROS_MESZAROS_DIR "PRIMAL1.mat", // Pass + // MAROS_MESZAROS_DIR "PRIMAL2.mat", // Pass + // MAROS_MESZAROS_DIR "PRIMAL3.mat", // Pass + // MAROS_MESZAROS_DIR "PRIMAL4.mat", // Skipping + // MAROS_MESZAROS_DIR "PRIMALC1.mat", // Fail: + // Values nan (primal residual 0, dual residual nan, iter 10000, polish 2, + // polish failed, mu updates 11) n: 230 n_eq+n_in: 239 + // MAROS_MESZAROS_DIR "PRIMALC2.mat", // Fail: + // Values nan (primal residual 0, dual residual nan, iter 10000, polish 2, + // polish failed, mu updates 8) n: 231 n_eq+n_in: 238 + // MAROS_MESZAROS_DIR "PRIMALC5.mat", // Fail: + // Values nan (primal residual 0, dual residual nan, iter 10000, polish 2, + // polish failed, mu updates 9) n: 287 n_eq+n_in: 295 + // MAROS_MESZAROS_DIR "PRIMALC8.mat", // Fail: + // Values nan (primal residual 0, dual residual nan, iter 10000, polish 2, + // polish failed, mu updates 8) n: 520 n_eq+n_in: 528 + // MAROS_MESZAROS_DIR "Q25FV47.mat", // Skipping + // MAROS_MESZAROS_DIR "QADLITTL.mat", // Fail: + // Values nan (primal residual huge, dual residual nan, iter 10000, polish + // 2, polish failed, mu updates 4) n: 97 n_eq+n_in: 153 + // MAROS_MESZAROS_DIR "QAFIRO.mat", // Fail: + // Values nan (primal residual nan, dual residual nan, iter 10000, polish 2, + // polish failed, mu updates 3) n: 32 n_eq+n_in: 59 + // MAROS_MESZAROS_DIR "QBANDM.mat", // Pass + // MAROS_MESZAROS_DIR "QBEACONF.mat", // Pass + // MAROS_MESZAROS_DIR "QBORE3D.mat", // Fail: Does + // not converge enough (primal residual 1e-1, dual residual 8e-3, iter + // 10000, polish not run, mu updates 1) n: 315 n_eq+n_in: 548 + // MAROS_MESZAROS_DIR "QBRANDY.mat", // Fail: Does + // not converge enough (primal residual 7e-2, dual residual 4e-2, iter + // 10000, polish not run, mu updates 2) n: 249 n_eq+n_in: 469 + // MAROS_MESZAROS_DIR "QCAPRI.mat", // Fail: Does + // not converge well (primal residual 7e-3, dual residual 7e-1, iter 10000, + // polish not run, mu updates 12) n: 353 n_eq+n_in: 624 + // MAROS_MESZAROS_DIR "QE226.mat", // Fail: Does + // not converge enough (primal residual 2e-3, dual residual 2e-3, iter + // 10000, polish not run, mu updates 1) n: 282 n_eq+n_in: 505 + // MAROS_MESZAROS_DIR "QETAMACR.mat", // Skipping + // MAROS_MESZAROS_DIR "QFFFFF80.mat", // Skipping + // MAROS_MESZAROS_DIR "QFORPLAN.mat", // Fail: Does + // not converge enough (primal residual 1e-2, dual residual 1e-1, iter + // 10000, polish not run, mu updates 10) n: 421 n_eq+n_in: 582 + // MAROS_MESZAROS_DIR "QGFRDXPN.mat", // Skipping + // MAROS_MESZAROS_DIR "QGROW15.mat", // Fail: Does + // not converge well (primal residual 6e3, dual residual 2e-2, iter 10000, + // polish not run, mu updates 50) n: 645 n_eq+n_in: 945 + // MAROS_MESZAROS_DIR "QGROW22.mat", // Skipping + // MAROS_MESZAROS_DIR "QGROW7.mat", // Fail: Does + // not converge well (primal residual 8e3, dual residual 6e-1, iter 10000, + // polish not run, mu updates 64) n: 301 n_eq+n_in: 441 + // MAROS_MESZAROS_DIR "QISRAEL.mat", // Fail: Does + // not converge well (primal residual 4e2, dual residual 1e3, iter 10000, + // polish not run, mu updates 30) n: 142 n_eq+n_in: 316 + // MAROS_MESZAROS_DIR "QPCBLEND.mat", // Fail: + // Values nan (primal residual nan, dual residual nan, iter 10000, polish 2, + // polish failed, mu updates 3) n: 83 n_eq+n_in: 157 + // MAROS_MESZAROS_DIR "QPCBOEI1.mat", // Fail: Does + // not converge enough (primal residual 1e-1, dual residual 1e-3, iter + // 10000, polish not run, mu updates 1) n: 384 n_eq+n_in: 735 + // MAROS_MESZAROS_DIR "QPCBOEI2.mat", // Fail: Does + // not converge well (primal residual 1e-2, dual residual 1e1, iter 10000, + // polish not run, mu updates 4) n: 143 n_eq+n_in: 309 + // MAROS_MESZAROS_DIR "QPCSTAIR.mat", // Pass + // MAROS_MESZAROS_DIR "QPILOTNO.mat", // Skipping + // MAROS_MESZAROS_DIR "QPTEST.mat", // Pass + // MAROS_MESZAROS_DIR "QRECIPE.mat", // Pass + // MAROS_MESZAROS_DIR "QSC205.mat", // Fail: + // Values nan (primal residual nan, dual residual nan, iter 10000, polish 2, + // polish failed, mu updates 4) n: 203 n_eq+n_in: 408 + // MAROS_MESZAROS_DIR "QSCAGR25.mat", // Fail: Does + // not converge enough (primal residual 6e-2, dual residual 6e-2, iter + // 10000, polish not run, mu updates 3) n: 500 n_eq+n_in: 971 + // MAROS_MESZAROS_DIR "QSCAGR7.mat", // Fail: Does + // not converge enough (primal residual 9e-2, dual residual 1e0, iter 10000, + // polish not run, mu updates 12) n: 140 n_eq+n_in: 269 + // MAROS_MESZAROS_DIR "QSCFXM1.mat", // Fail: Does + // not converge enough (primal residual 3e-2, dual residual 4e-2, iter + // 10000, polish not run, mu updates 1) n: 457 n_eq+n_in: 787 + // MAROS_MESZAROS_DIR "QSCFXM2.mat", // Skipping + // MAROS_MESZAROS_DIR "QSCFXM3.mat", // Skipping + MAROS_MESZAROS_DIR + "QSCORPIO.mat", // Fail: Does not converge well and polish -> ?? (primal + // residual 2e-4, dual residual 7e-1, iter 10000, polish 2, + // polish failed, mu updates 18) n: 358 n_eq+n_in: 746 + // MAROS_MESZAROS_DIR "QSCRS8.mat", // Skipping + MAROS_MESZAROS_DIR + "QSCSD1.mat", // Fail: Does not converge well and polish -> ?? (primal + // residual 1e-2, dual residual 1e-2, iter 10000, polish 2, + // polish failed, mu updates 134) n: 760 n_eq+n_in: 837 + // MAROS_MESZAROS_DIR "QSCSD6.mat", // Skipping + // MAROS_MESZAROS_DIR "QSCSD8.mat", // Skipping + // MAROS_MESZAROS_DIR "QSCTAP1.mat", // Fail: Does + // not converge well (primal residual 5e-3, dual residual 1e0, iter 10000, + // polish not run, mu updates 16) n: 480 n_eq+n_in: 780 + // MAROS_MESZAROS_DIR "QSCTAP2.mat", // Skipping + // MAROS_MESZAROS_DIR "QSCTAP3.mat", + // MAROS_MESZAROS_DIR "QSEBA.mat", // Skipping + // MAROS_MESZAROS_DIR "QSHARE1B.mat", // Fail: Does + // not converge well (primal residual 3e0, dual residual 3e-3, iter 10000, + // polish not run, mu updates 4) n: 225 n_eq+n_in: 342 + // MAROS_MESZAROS_DIR "QSHARE2B.mat", // Fail: Does + // not converge enough (primal residual 1e-1, dual residual 8e-1, iter + // 10000, polish not run, mu updates 4) n: 79 n_eq+n_in: 175 + // MAROS_MESZAROS_DIR "QSHELL.mat", // Skipping + // MAROS_MESZAROS_DIR "QSHIP04L.mat", // Skipping + // MAROS_MESZAROS_DIR "QSHIP04S.mat", // Skipping + // MAROS_MESZAROS_DIR "QSHIP08L.mat", // Skipping + // MAROS_MESZAROS_DIR "QSHIP08S.mat", // Skipping + // MAROS_MESZAROS_DIR "QSHIP12L.mat", // Skipping + // MAROS_MESZAROS_DIR "QSHIP12S.mat", // Skipping + // MAROS_MESZAROS_DIR "QSIERRA.mat", // Skipping + // MAROS_MESZAROS_DIR "QSTAIR.mat", // Fail: Does + // not converge enough (primal residual 2e-2, dual residual 3e-1, iter + // 10000, polish not run, mu updates 13) n: 467 n_eq+n_in: 823 + // MAROS_MESZAROS_DIR "QSTANDAT.mat", // Skipping + // MAROS_MESZAROS_DIR "S268.mat", // Pass + // MAROS_MESZAROS_DIR "STADAT1.mat", // Skipping + // MAROS_MESZAROS_DIR "STADAT2.mat", // Skipping + // MAROS_MESZAROS_DIR "STADAT3.mat", // Skipping + // MAROS_MESZAROS_DIR "STCQP1.mat", // Skipping + // MAROS_MESZAROS_DIR "STCQP2.mat", // Skipping + // MAROS_MESZAROS_DIR "TAME.mat", // Pass + // MAROS_MESZAROS_DIR "UBH1.mat", // Skipping + // MAROS_MESZAROS_DIR "VALUES.mat", // Pass + // MAROS_MESZAROS_DIR "YAO.mat", // Skipping + // MAROS_MESZAROS_DIR "ZECEVIC2.mat", // Pass +}; + +TEST_CASE("dense maros meszaros using the api") +{ + using T = double; + using isize = proxqp::utils::isize; + proxsuite::proxqp::Timer timer; + T elapsed_time = 0.0; + + for (auto const* file : files) { + auto qp = load_qp(file); + isize n = qp.P.rows(); + isize n_eq_in = qp.A.rows(); + + const bool skip = n > 1000 || n_eq_in > 1000; + if (skip) { + std::cout << " path: " << qp.filename << " n: " << n + << " n_eq+n_in: " << n_eq_in << " - skipping" << std::endl; + } else { + std::cout << " path: " << qp.filename << " n: " << n + << " n_eq+n_in: " << n_eq_in << std::endl; + } + + if (!skip) { + + auto preprocessed = preprocess_qp(qp); + auto& H = preprocessed.H; + auto& A = preprocessed.A; + auto& C = preprocessed.C; + auto& g = preprocessed.g; + auto& b = preprocessed.b; + auto& u = preprocessed.u; + auto& l = preprocessed.l; + + isize dim = H.rows(); + isize n_eq = A.rows(); + isize n_in = C.rows(); + timer.stop(); + timer.start(); + pod::QP qp{ + dim, n_eq, n_in, false, proxsuite::proxqp::DenseBackend::PrimalDualLDLT + }; // creating QP object + + qp.settings.default_mu_eq = T(1.E-2); + qp.settings.default_mu_in = T(1.E1); + + qp.init(H, g, A, b, C, l, u); + + qp.settings.eps_abs = 2e-8; + qp.settings.eps_rel = 0; + qp.settings.eps_primal_inf = 1e-12; + qp.settings.eps_dual_inf = 1e-12; + + { + qp.settings.max_iter = 10000; + qp.settings.verbose = false; + } + + auto& eps = qp.settings.eps_abs; + + // for (size_t it = 0; it < 2; ++it) { + size_t it = 0; // Just test first solve for a first glance + { + if (it > 0) + qp.settings.initial_guess = proxsuite::proxqp::InitialGuessStatus:: + WARM_START_WITH_PREVIOUS_RESULT; + + qp.solve(); + const auto& x = qp.results.x; + const auto& y = qp.results.y; + const auto& z = qp.results.z; + + T prim_eq = proxqp::dense::infty_norm(A * x - b); + T prim_in = + proxqp::dense::infty_norm(helpers::positive_part(C * x - u) + + helpers::negative_part(C * x - l)); + std::cout << "primal residual " << std::max(prim_eq, prim_in) + << std::endl; + std::cout << "dual residual " + << proxqp::dense::infty_norm(H * x + g + A.transpose() * y + + C.transpose() * z) + << std::endl; + std::cout << "admm iter " << qp.results.info.iter_ext << std::endl; + { + std::cout << "polish calls " << qp.results.info.polish_calls + << std::endl; + switch (qp.results.info.polish_status) { + case pp::PolishStatus::POLISH_NOT_RUN: { + std::cout << "polish not run" << std::endl; + break; + } + case pp::PolishStatus::POLISH_SUCCEED: { + std::cout << "polish succeeded" << std::endl; + break; + } + case pp::PolishStatus::POLISH_FAILED: { + std::cout << "polish failed" << std::endl; + break; + } + case pp::PolishStatus::POLISH_NO_ACTIVE_SET_FOUND: { + std::cout << "polish no active set found" << std::endl; + break; + } + } + std::cout << "mu updates " << qp.results.info.mu_updates << std::endl; + } + CHECK(proxqp::dense::infty_norm(H * x + g + A.transpose() * y + + C.transpose() * z) < 2 * eps); + CHECK(proxqp::dense::infty_norm(A * x - b) > -eps); + CHECK((C * x - l).minCoeff() > -eps); + CHECK((C * x - u).maxCoeff() < eps); + + if (it > 0) { + CHECK(qp.results.info.iter_ext == 0); + } + } + timer.stop(); + elapsed_time += timer.elapsed().user; + } + } + std::cout << "timings total : \t" << elapsed_time * 1e-3 << "ms" << std::endl; +} diff --git a/test/src/osqp_dense_qp_eq.cpp b/test/src/osqp_dense_qp_eq.cpp new file mode 100644 index 000000000..5dbbd5905 --- /dev/null +++ b/test/src/osqp_dense_qp_eq.cpp @@ -0,0 +1,329 @@ +// +// Copyright (c) 2022 - 2024 INRIA +// +#include "proxsuite/proxqp/status.hpp" +#include +#include +#include +#include +#include +#include +#include + +using T = double; +using namespace proxsuite; +namespace pod = proxsuite::osqp::dense; + +DOCTEST_TEST_CASE("qp: start from solution using the wrapper framework") +{ + proxqp::isize dim = 30; + proxqp::isize n_eq = 6; + proxqp::isize n_in = 0; + T sparsity_factor = 0.15; + T strong_convexity_factor(1.e-2); + std::cout << "---testing sparse random strongly convex qp with equality " + "constraints and starting at the solution using the wrapper " + "framework---" + << std::endl; + proxqp::utils::rand::set_seed(1); + auto H = ::proxsuite::proxqp::utils::rand:: + sparse_positive_definite_rand_not_compressed( + dim, strong_convexity_factor, sparsity_factor); + auto A = + ::proxsuite::proxqp::utils::rand::sparse_matrix_rand_not_compressed( + n_eq, dim, sparsity_factor); + auto solution = ::proxsuite::proxqp::utils::rand::vector_rand(dim + n_eq); + auto primal_solution = solution.topRows(dim); + auto dual_solution = solution.bottomRows(n_eq); + auto b = A * primal_solution; + auto g = -H * primal_solution - A.transpose() * dual_solution; + auto C = + ::proxsuite::proxqp::utils::rand::sparse_matrix_rand_not_compressed( + 0, dim, sparsity_factor); + Eigen::Matrix dual_init_in(n_in); + Eigen::Matrix u(0); + Eigen::Matrix l(0); + dual_init_in.setZero(); + T eps_abs = T(1e-9); + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.initial_guess = proxsuite::proxqp::InitialGuessStatus::WARM_START; + qp.settings.default_mu_eq = T(1.E-2); + qp.settings.default_mu_in = T(1.E1); + qp.init(H, g, A, b, C, l, u); + qp.solve(primal_solution, dual_solution, dual_init_in); + + DOCTEST_CHECK((A * qp.results.x - b).lpNorm() <= eps_abs); + DOCTEST_CHECK((H * qp.results.x + g + A.transpose() * qp.results.y) + .lpNorm() <= eps_abs); +} +DOCTEST_TEST_CASE("sparse random strongly convex qp with equality constraints " + "and increasing dimension with the wrapper API") +{ + + std::cout << "---testing sparse random strongly convex qp with equality " + "constraints and increasing dimension with the wrapper API---" + << std::endl; + T sparsity_factor = 0.15; + T eps_abs = T(1e-9); + proxqp::utils::rand::set_seed(1); + for (proxqp::isize dim = 10; dim < 1000; dim += 100) { + + proxqp::isize n_eq(dim / 2); + proxqp::isize n_in(0); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.default_mu_eq = T(1.E-2); + qp.settings.default_mu_in = T(1.E1); + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.u, + qp_random.l); + qp.solve(); + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using wrapper API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "number of iterations (admm): " << qp.results.info.iter_ext + << std::endl; + if (qp.settings.polish && + !(qp.results.info.polish_status == + proxqp::PolishStatus::POLISH_NO_ACTIVE_SET_FOUND || + qp.results.info.polish_status == + proxqp::PolishStatus::POLISH_NOT_RUN)) { + std::cout << "number of iterations (polishing): " + << qp.settings.polish_refine_iter << std::endl; + } + } +} +DOCTEST_TEST_CASE("linear problem with equality with equality constraints and " + "linar cost and increasing dimension using wrapper API") +{ + + std::cout << "---testing linear problem with equality constraints and " + "increasing dimension using wrapper API---" + << std::endl; + T sparsity_factor = 0.15; + T eps_abs = T(1e-9); + proxqp::utils::rand::set_seed(1); + for (proxqp::isize dim = 10; dim < 1000; dim += 100) { + + proxqp::isize n_eq(dim / 2); + proxqp::isize n_in(0); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + qp_random.H.setZero(); + auto y_sol = proxqp::utils::rand::vector_rand( + n_eq); // make sure the LP is bounded within the feasible set + qp_random.g = -qp_random.A.transpose() * y_sol; + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.default_mu_eq = T(1.E-2); + qp.settings.default_mu_in = T(1.E1); + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using wrapper API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "number of iterations (admm): " << qp.results.info.iter_ext + << std::endl; + if (qp.settings.polish && + !(qp.results.info.polish_status == + proxqp::PolishStatus::POLISH_NO_ACTIVE_SET_FOUND || + qp.results.info.polish_status == + proxqp::PolishStatus::POLISH_NOT_RUN)) { + std::cout << "number of iterations (polishing): " + << qp.settings.polish_refine_iter << std::endl; + } + } +} + +DOCTEST_TEST_CASE("linear problem with equality with equality constraints and " + "linear cost and increasing dimension using wrapper API and " + "the dedicated LP interface") +{ + + std::cout + << "---testing LP interface for solving linear problem with " + "equality constraints and increasing dimension using wrapper API---" + << std::endl; + T sparsity_factor = 0.15; + T eps_abs = T(1e-9); + proxqp::utils::rand::set_seed(1); + for (proxqp::isize dim = 10; dim < 1000; dim += 100) { + + proxqp::isize n_eq(dim / 2); + proxqp::isize n_in(0); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + qp_random.H.setZero(); + auto y_sol = proxqp::utils::rand::vector_rand( + n_eq); // make sure the LP is bounded within the feasible set + qp_random.g = -qp_random.A.transpose() * y_sol; + + pod::QP qp{ + dim, n_eq, n_in, proxqp::HessianType::Zero + }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.default_mu_eq = T(1.E-2); + qp.settings.default_mu_in = T(1.E1); + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using wrapper API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "number of iterations (admm): " << qp.results.info.iter_ext + << std::endl; + if (qp.settings.polish && + !(qp.results.info.polish_status == + proxqp::PolishStatus::POLISH_NO_ACTIVE_SET_FOUND || + qp.results.info.polish_status == + proxqp::PolishStatus::POLISH_NOT_RUN)) { + std::cout << "number of iterations (polishing): " + << qp.settings.polish_refine_iter << std::endl; + } + } +} + +DOCTEST_TEST_CASE("infeasible qp") +{ + // (x1- 9)^2 + (x2-6)^2 + // s.t. + // x1 <= 10 + // x2 <= 10 + // x1 >= 20 + Eigen::Matrix H; + H << 1.0, 0.0, 0.0, 1.0; + H = 2 * H; + + Eigen::Matrix g; + g << -18.0, -12.0; + + Eigen::Matrix C; + C << 1, 0, // x1 <= 10 + 0, 1, // x2 <= 10 + -1, 0; // x1 >= 20 + + Eigen::Matrix u; + u << 10, 10, -20; + + int n = H.rows(); + int n_in = C.rows(); + int n_eq = 0; + + Eigen::Matrix l = + Eigen::Matrix::Constant( + n_in, -std::numeric_limits::infinity()); + + pod::QP qp(n, n_eq, n_in); + qp.settings.default_mu_eq = T(1.E-2); + qp.settings.default_mu_in = T(1.E1); + qp.init(H, g, nullopt, nullopt, C, l, u); + qp.settings.eps_rel = 0.; + qp.settings.eps_abs = 1e-9; + + qp.solve(); + + DOCTEST_CHECK(qp.results.info.status == + proxsuite::proxqp::QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE); +} + +DOCTEST_TEST_CASE("dual infeasible qp equality constraints") +{ + Eigen::Matrix H; + H << 0.0, 0.0, 0.0, 0.0; + H = 2 * H; + + Eigen::Matrix g; + g << 1.0, -1.0; + + Eigen::Matrix A; + A << 1, 1, 1, 1; + + Eigen::Matrix b; + b << 1, 1; + + int n = H.rows(); + int n_in = 0; + int n_eq = A.rows(); + + // osqp::dense::QP qp{ n, n_eq, n_in }; // creating QP object + bool box_constraints = false; + pod::QP qp{ + n, n_eq, n_in, box_constraints, proxqp::DenseBackend::PrimalDualLDLT + }; // creating QP object + qp.settings.default_mu_eq = T(1.E-2); + qp.settings.default_mu_in = T(1.E1); + qp.init(H, g, A, b, nullopt, nullopt, nullopt); + qp.settings.eps_rel = 0.; + qp.settings.eps_abs = 1e-9; + + qp.solve(); + + DOCTEST_CHECK(qp.results.info.status == + proxsuite::proxqp::QPSolverOutput::PROXQP_DUAL_INFEASIBLE); +} \ No newline at end of file diff --git a/test/src/osqp_dense_qp_solve.cpp b/test/src/osqp_dense_qp_solve.cpp new file mode 100644 index 000000000..6e9d9f2b1 --- /dev/null +++ b/test/src/osqp_dense_qp_solve.cpp @@ -0,0 +1,435 @@ +// +// Copyright (c) 2022 INRIA +// +#include +#include +#include +#include +#include +#include +#include + +using T = double; +using namespace proxsuite; + +namespace pp = proxsuite::proxqp; +namespace ppd = proxsuite::proxqp::dense; +namespace pod = proxsuite::osqp::dense; + +DOCTEST_TEST_CASE("proxqp::dense: test init with fixed sizes matrices") +{ + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + pp::utils::rand::set_seed(1); + ppd::isize dim = 10; + + ppd::isize n_eq(5), n_in(2); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + Eigen::Matrix H = qp.H; + Eigen::Matrix g = qp.g; + Eigen::Matrix A = qp.A; + Eigen::Matrix b = qp.b; + Eigen::Matrix C = qp.C; + Eigen::Matrix l = qp.l; + Eigen::Matrix u = qp.u; + + { + pp::Results results = pod::solve(H, + g, + A, + b, + C, + l, + u, + nullopt, + nullopt, + nullopt, + eps_abs, + 0, + nullopt, + T(1E-2), + T(1E1)); + + T pri_res = std::max((qp.A * results.x - qp.b).lpNorm(), + (helpers::positive_part(qp.C * results.x - qp.u) + + helpers::negative_part(qp.C * results.x - qp.l)) + .lpNorm()); + T dua_res = (qp.H * results.x + qp.g + qp.A.transpose() * results.y + + qp.C.transpose() * results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration (admm): " << results.info.iter_ext + << std::endl; + std::cout << "setup timing " << results.info.setup_time << " solve time " + << results.info.solve_time << std::endl; + } + + { + pod::QP qp_problem(dim, n_eq, 0); + qp_problem.settings.default_mu_eq = T(1.E-2); + qp_problem.settings.default_mu_in = T(1.E1); + qp_problem.init(H, g, A, b, nullopt, nullopt, nullopt); + qp_problem.settings.eps_abs = eps_abs; + qp_problem.solve(); + + const pp::Results& results = qp_problem.results; + + T pri_res = (qp.A * results.x - qp.b).lpNorm(); + T dua_res = (qp.H * results.x + qp.g + qp.A.transpose() * results.y) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration (admm): " << results.info.iter_ext + << std::endl; + std::cout << "setup timing " << results.info.setup_time << " solve time " + << results.info.solve_time << std::endl; + } +} + +DOCTEST_TEST_CASE("sparse random strongly convex qp with equality and " + "inequality constraints: test solve function") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test solve function---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + pp::utils::rand::set_seed(1); + ppd::isize dim = 10; + + ppd::isize n_eq(dim / 4); + ppd::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + pp::Results results = pod::solve(qp.H, + qp.g, + qp.A, + qp.b, + qp.C, + qp.l, + qp.u, + nullopt, + nullopt, + nullopt, + eps_abs, + 0, + nullopt, + T(1E-2), + T(1E1)); + + T pri_res = std::max((qp.A * results.x - qp.b).lpNorm(), + (helpers::positive_part(qp.C * results.x - qp.u) + + helpers::negative_part(qp.C * results.x - qp.l)) + .lpNorm()); + T dua_res = (qp.H * results.x + qp.g + qp.A.transpose() * results.y + + qp.C.transpose() * results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration (admm): " << results.info.iter_ext + << std::endl; + std::cout << "setup timing " << results.info.setup_time << " solve time " + << results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE("sparse random strongly convex qp with equality and " + "inequality constraints: test solve with different rho " + "value") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test solve with different rho " + "value---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + pp::utils::rand::set_seed(1); + ppd::isize dim = 10; + + ppd::isize n_eq(dim / 4); + ppd::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + pp::Results results = pod::solve(qp.H, + qp.g, + qp.A, + qp.b, + qp.C, + qp.l, + qp.u, + nullopt, + nullopt, + nullopt, + eps_abs, + 0, + T(1.E-7), + T(1E-2), + T(1E1)); + DOCTEST_CHECK(results.info.rho == T(1.E-7)); + T pri_res = std::max((qp.A * results.x - qp.b).lpNorm(), + (helpers::positive_part(qp.C * results.x - qp.u) + + helpers::negative_part(qp.C * results.x - qp.l)) + .lpNorm()); + T dua_res = (qp.H * results.x + qp.g + qp.A.transpose() * results.y + + qp.C.transpose() * results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration (admm): " << results.info.iter_ext + << std::endl; + std::cout << "setup timing " << results.info.setup_time << " solve time " + << results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "sparse random strongly convex qp with equality and " + "inequality constraints: test solve with different mu_eq and mu_in values") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test solve with different mu_eq and " + "mu_in values---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + pp::utils::rand::set_seed(1); + ppd::isize dim = 10; + + ppd::isize n_eq(dim / 4); + ppd::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + pp::Results results = pod::solve(qp.H, + qp.g, + qp.A, + qp.b, + qp.C, + qp.l, + qp.u, + nullopt, + nullopt, + nullopt, + eps_abs, + 0, + nullopt, + T(1.E-1), + T(1.E0)); + // Note: + // Different alternative values than for proxqp, due to different + // suggestions for default_mu_eq and default_mu_in between proxqp paper + // and osqp paper - but same update (*10 for mu_eq and /10 for mu_in) + T pri_res = std::max((qp.A * results.x - qp.b).lpNorm(), + (helpers::positive_part(qp.C * results.x - qp.u) + + helpers::negative_part(qp.C * results.x - qp.l)) + .lpNorm()); + T dua_res = (qp.H * results.x + qp.g + qp.A.transpose() * results.y + + qp.C.transpose() * results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration (admm): " << results.info.iter_ext + << std::endl; + std::cout << "setup timing " << results.info.setup_time << " solve time " + << results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE("sparse random strongly convex qp with equality and " + "inequality constraints: test warm starting") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test warm starting---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + pp::utils::rand::set_seed(1); + ppd::isize dim = 10; + + ppd::isize n_eq(dim / 4); + ppd::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + auto x_wm = pp::utils::rand::vector_rand(dim); + auto y_wm = pp::utils::rand::vector_rand(n_eq); + auto z_wm = pp::utils::rand::vector_rand(n_in); + pp::Results results = pod::solve(qp.H, + qp.g, + qp.A, + qp.b, + qp.C, + qp.l, + qp.u, + x_wm, + y_wm, + z_wm, + eps_abs, + 0, + nullopt, + T(1E-2), + T(1E1)); + T pri_res = std::max((qp.A * results.x - qp.b).lpNorm(), + (helpers::positive_part(qp.C * results.x - qp.u) + + helpers::negative_part(qp.C * results.x - qp.l)) + .lpNorm()); + T dua_res = (qp.H * results.x + qp.g + qp.A.transpose() * results.y + + qp.C.transpose() * results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration (admm): " << results.info.iter_ext + << std::endl; + std::cout << "setup timing " << results.info.setup_time << " solve time " + << results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE("sparse random strongly convex qp with equality and " + "inequality constraints: test verbose = true") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test verbose = true ---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + pp::utils::rand::set_seed(1); + ppd::isize dim = 10; + + ppd::isize n_eq(dim / 4); + ppd::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + bool verbose = true; + pp::Results results = pod::solve(qp.H, + qp.g, + qp.A, + qp.b, + qp.C, + qp.l, + qp.u, + nullopt, + nullopt, + nullopt, + eps_abs, + 0, + nullopt, + T(1E-2), + T(1E1), + verbose); + T pri_res = std::max((qp.A * results.x - qp.b).lpNorm(), + (helpers::positive_part(qp.C * results.x - qp.u) + + helpers::negative_part(qp.C * results.x - qp.l)) + .lpNorm()); + T dua_res = (qp.H * results.x + qp.g + qp.A.transpose() * results.y + + qp.C.transpose() * results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration (admm): " << results.info.iter_ext + << std::endl; + std::cout << "setup timing " << results.info.setup_time << " solve time " + << results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE("sparse random strongly convex qp with equality and " + "inequality constraints: test no initial guess") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test no initial guess ---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + pp::utils::rand::set_seed(1); + ppd::isize dim = 10; + + ppd::isize n_eq(dim / 4); + ppd::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + pp::InitialGuessStatus initial_guess = + pp::InitialGuessStatus::NO_INITIAL_GUESS; + pp::Results results = pod::solve(qp.H, + qp.g, + qp.A, + qp.b, + qp.C, + qp.l, + qp.u, + nullopt, + nullopt, + nullopt, + eps_abs, + 0, + nullopt, + T(1E-2), + T(1E1), + nullopt, + true, + true, + nullopt, + initial_guess); + T pri_res = std::max((qp.A * results.x - qp.b).lpNorm(), + (helpers::positive_part(qp.C * results.x - qp.u) + + helpers::negative_part(qp.C * results.x - qp.l)) + .lpNorm()); + T dua_res = (qp.H * results.x + qp.g + qp.A.transpose() * results.y + + qp.C.transpose() * results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration (admm): " << results.info.iter_ext + << std::endl; + std::cout << "setup timing " << results.info.setup_time << " solve time " + << results.info.solve_time << std::endl; +} diff --git a/test/src/osqp_dense_qp_with_eq_and_in.cpp b/test/src/osqp_dense_qp_with_eq_and_in.cpp new file mode 100644 index 000000000..0f35c802e --- /dev/null +++ b/test/src/osqp_dense_qp_with_eq_and_in.cpp @@ -0,0 +1,336 @@ +// +// Copyright (c) 2022 INRIA +// +#include +#include +#include +#include +#include +#include +#include + +using T = double; +using namespace proxsuite; +namespace pod = proxsuite::osqp::dense; + +// DOCTEST_TEST_CASE( +// "sparse random strongly convex qp with equality and inequality constraints" +// "and increasing dimension using wrapper API") +// { + +// std::cout +// << "---testing sparse random strongly convex qp with equality and " +// "inequality constraints and increasing dimension using wrapper API---" +// << std::endl; +// T sparsity_factor = 0.15; +// T eps_abs = T(1e-9); +// proxqp::utils::rand::set_seed(1); +// // for (proxqp::isize dim = 10; dim < 1000; dim += 100) { +// for (proxqp::isize dim = 10; dim < 30; dim += 7) { + +// proxqp::isize n_eq(dim / 4); +// proxqp::isize n_in(dim / 4); +// T strong_convexity_factor(1.e-2); +// proxqp::dense::Model qp_random = +// proxqp::utils::dense_strongly_convex_qp( +// dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + +// pod::QP qp{ dim, n_eq, n_in }; // creating QP object +// qp.settings.eps_abs = eps_abs; +// qp.settings.eps_rel = 0; +// qp.settings.default_mu_eq = T(1.E-2); +// qp.settings.default_mu_in = T(1.E1); +// qp.init(qp_random.H, +// qp_random.g, +// qp_random.A, +// qp_random.b, +// qp_random.C, +// qp_random.l, +// qp_random.u); +// qp.solve(); + +// T pri_res = std::max( +// (qp_random.A * qp.results.x - qp_random.b).lpNorm(), +// (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + +// helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) +// .lpNorm()); +// T dua_res = (qp_random.H * qp.results.x + qp_random.g + +// qp_random.A.transpose() * qp.results.y + +// qp_random.C.transpose() * qp.results.z) +// .lpNorm(); +// DOCTEST_CHECK(pri_res <= eps_abs); +// DOCTEST_CHECK(dua_res <= eps_abs); + +// std::cout << "------using API solving qp with dim: " << dim +// << " neq: " << n_eq << " nin: " << n_in << std::endl; +// std::cout << "primal residual: " << pri_res << std::endl; +// std::cout << "dual residual: " << dua_res << std::endl; +// std::cout << "number of iterations (admm): " << qp.results.info.iter_ext +// << std::endl; +// if (qp.settings.polish && +// !(qp.results.info.polish_status == +// proxqp::PolishStatus::POLISH_NO_ACTIVE_SET_FOUND || +// qp.results.info.polish_status == +// proxqp::PolishStatus::POLISH_NOT_RUN)) { +// std::cout << "number of iterations (polishing): " +// << qp.settings.polish_refine_iter << std::endl; +// } +// } +// } + +// DOCTEST_TEST_CASE("sparse random strongly convex qp with box inequality " +// "constraints and increasing dimension using the API") +// { + +// std::cout +// << "---testing sparse random strongly convex qp with box inequality " +// "constraints and increasing dimension using the API---" +// << std::endl; +// T sparsity_factor = 0.15; +// T eps_abs = T(1e-9); +// proxqp::utils::rand::set_seed(1); +// // for (proxqp::isize dim = 10; dim < 1000; dim += 100) { +// for (proxqp::isize dim = 10; dim < 30; dim += 7) { + +// proxqp::isize n_eq(0); +// proxqp::isize n_in(dim); +// T strong_convexity_factor(1.e-2); +// proxqp::dense::Model qp_random = +// proxqp::utils::dense_box_constrained_qp( +// dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); +// pod::QP qp{ dim, n_eq, n_in }; // creating QP object +// qp.settings.eps_abs = eps_abs; +// qp.settings.eps_rel = 0; +// qp.settings.default_mu_eq = T(1.E-2); +// qp.settings.default_mu_in = T(1.E1); +// qp.init(qp_random.H, +// qp_random.g, +// qp_random.A, +// qp_random.b, +// qp_random.C, +// qp_random.l, +// qp_random.u); +// qp.solve(); +// T pri_res = std::max( +// (qp_random.A * qp.results.x - qp_random.b).lpNorm(), +// (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + +// helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) +// .lpNorm()); +// T dua_res = (qp_random.H * qp.results.x + qp_random.g + +// qp_random.A.transpose() * qp.results.y + +// qp_random.C.transpose() * qp.results.z) +// .lpNorm(); +// DOCTEST_CHECK(pri_res <= eps_abs); +// DOCTEST_CHECK(dua_res <= eps_abs); + +// std::cout << "------solving qp with dim: " << dim << " neq: " << n_eq +// << " nin: " << n_in << std::endl; +// std::cout << "primal residual: " << pri_res << std::endl; +// std::cout << "dual residual: " << dua_res << std::endl; +// std::cout << "number of iterations: " << qp.results.info.iter_ext +// << std::endl; +// if (qp.settings.polish && +// !(qp.results.info.polish_status == +// proxqp::PolishStatus::POLISH_NO_ACTIVE_SET_FOUND || +// qp.results.info.polish_status == +// proxqp::PolishStatus::POLISH_NOT_RUN)) { +// std::cout << "number of iterations (polishing): " +// << qp.settings.polish_refine_iter << std::endl; +// } +// } +// } + +// DOCTEST_TEST_CASE("sparse random not strongly convex qp with inequality " +// "constraints and increasing dimension using the API") +// { + +// std::cout +// << "---testing sparse random not strongly convex qp with inequality " +// "constraints and increasing dimension using the API---" +// << std::endl; +// T sparsity_factor = 0.15; +// T eps_abs = T(1e-9); +// proxqp::utils::rand::set_seed(1); +// // for (proxqp::isize dim = 10; dim < 1000; dim += 100) { +// for (proxqp::isize dim = 10; dim < 30; dim += 7) { +// proxqp::isize n_in(dim / 2); +// proxqp::isize n_eq(0); +// proxqp::dense::Model qp_random = +// proxqp::utils::dense_not_strongly_convex_qp( +// dim, n_eq, n_in, sparsity_factor); + +// pod::QP qp{ dim, n_eq, n_in }; // creating QP object +// qp.settings.eps_abs = eps_abs; +// qp.settings.eps_rel = 0; +// qp.settings.default_mu_eq = T(1.E-2); +// qp.settings.default_mu_in = T(1.E1); +// qp.init(qp_random.H, +// qp_random.g, +// qp_random.A, +// qp_random.b, +// qp_random.C, +// qp_random.l, +// qp_random.u); +// qp.solve(); +// T pri_res = std::max( +// (qp_random.A * qp.results.x - qp_random.b).lpNorm(), +// (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + +// helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) +// .lpNorm()); +// T dua_res = (qp_random.H * qp.results.x + qp_random.g + +// qp_random.A.transpose() * qp.results.y + +// qp_random.C.transpose() * qp.results.z) +// .lpNorm(); +// DOCTEST_CHECK(pri_res <= eps_abs); +// DOCTEST_CHECK(dua_res <= eps_abs); + +// std::cout << "------solving qp with dim: " << dim << " neq: " << n_eq +// << " nin: " << n_in << std::endl; +// std::cout << "primal residual: " << pri_res << std::endl; +// std::cout << "dual residual: " << dua_res << std::endl; +// std::cout << "number of iterations (admm): " << qp.results.info.iter_ext +// << std::endl; +// if (qp.settings.polish && +// !(qp.results.info.polish_status == +// proxqp::PolishStatus::POLISH_NO_ACTIVE_SET_FOUND || +// qp.results.info.polish_status == +// proxqp::PolishStatus::POLISH_NOT_RUN)) { +// std::cout << "number of iterations (polishing): " +// << qp.settings.polish_refine_iter << std::endl; +// } +// } +// } + +DOCTEST_TEST_CASE("sparse random strongly convex qp with degenerate inequality " + "constraints and increasing dimension using the API") +{ + + std::cout + << "---testing sparse random strongly convex qp with degenerate " + "inequality constraints and increasing dimension using the API---" + << std::endl; + T sparsity_factor = 0.45; + T eps_abs = T(1e-9); + T strong_convexity_factor(1e-2); + proxqp::utils::rand::set_seed(1); + // for (proxqp::isize dim = 10; dim < 1000; dim += 100) { + // for (proxqp::isize dim = 10; dim < 110; dim += 10) { + proxqp::isize dim = 50; + { + std::cout << "current dim: " << dim << std::endl; + proxqp::isize m(dim / 4); + proxqp::isize n_in(2 * m); + proxqp::isize n_eq(0); + proxqp::dense::Model qp_random = proxqp::utils::dense_degenerate_qp( + dim, + n_eq, + m, // it n_in = 2 * m, it doubles the inequality constraints + sparsity_factor, + strong_convexity_factor); + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.default_mu_eq = T(1.E-2); + qp.settings.default_mu_in = T(1.E1); + qp.settings.high_accuracy = false; + qp.settings.verbose = true; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + DOCTEST_CHECK(qp.results.info.status == + proxqp::QPSolverOutput::PROXQP_SOLVED); + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------solving qp with dim: " << dim << " neq: " << n_eq + << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "number of iterations (admm): " << qp.results.info.iter_ext + << std::endl; + if (qp.settings.polish && + !(qp.results.info.polish_status == + proxqp::PolishStatus::POLISH_NO_ACTIVE_SET_FOUND || + qp.results.info.polish_status == + proxqp::PolishStatus::POLISH_NOT_RUN)) { + std::cout << "number of iterations (polishing): " + << qp.settings.polish_refine_iter << std::endl; + } + } +} + +// DOCTEST_TEST_CASE("linear problem with equality inequality constraints and " +// "increasing dimension using the API") +// { +// srand(1); +// std::cout << "---testing linear problem with inequality constraints and " +// "increasing dimension using the API---" +// << std::endl; +// T sparsity_factor = 0.15; +// T eps_abs = T(1e-9); +// proxqp::utils::rand::set_seed(1); +// for (proxqp::isize dim = 10; dim < 1000; dim += 100) { +// proxqp::isize n_in(dim / 2); +// proxqp::isize n_eq(0); +// proxqp::dense::Model qp_random = +// proxqp::utils::dense_not_strongly_convex_qp( +// dim, n_eq, n_in, sparsity_factor); +// qp_random.H.setZero(); +// auto z_sol = proxqp::utils::rand::vector_rand(n_in); +// qp_random.g = -qp_random.C.transpose() * +// z_sol; // make sure LP is bounded within the feasible set +// pod::QP qp{ dim, n_eq, n_in }; // creating QP object +// qp.settings.eps_abs = eps_abs; +// qp.settings.eps_rel = 0; +// qp.settings.default_mu_eq = T(1.E-2); +// qp.settings.default_mu_in = T(1.E1); +// qp.init(qp_random.H, +// qp_random.g, +// qp_random.A, +// qp_random.b, +// qp_random.C, +// qp_random.l, +// qp_random.u); +// qp.solve(); +// T pri_res = std::max( +// (qp_random.A * qp.results.x - qp_random.b).lpNorm(), +// (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + +// helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) +// .lpNorm()); +// T dua_res = (qp_random.H * qp.results.x + qp_random.g + +// qp_random.A.transpose() * qp.results.y + +// qp_random.C.transpose() * qp.results.z) +// .lpNorm(); +// DOCTEST_CHECK(pri_res <= eps_abs); +// DOCTEST_CHECK(dua_res <= eps_abs); + +// std::cout << "------solving qp with dim: " << dim << " neq: " << n_eq +// << " nin: " << n_in << std::endl; +// std::cout << "primal residual: " << pri_res << std::endl; +// std::cout << "dual residual: " << dua_res << std::endl; +// std::cout << "number of iterations (admm): " << qp.results.info.iter_ext +// << std::endl; +// if (qp.settings.polish && +// !(qp.results.info.polish_status == +// proxqp::PolishStatus::POLISH_NO_ACTIVE_SET_FOUND || +// qp.results.info.polish_status == +// proxqp::PolishStatus::POLISH_NOT_RUN)) { +// std::cout << "number of iterations (polishing): " +// << qp.settings.polish_refine_iter << std::endl; +// } +// } +// } diff --git a/test/src/osqp_dense_qp_wrapper.cpp b/test/src/osqp_dense_qp_wrapper.cpp new file mode 100644 index 000000000..5002fef9e --- /dev/null +++ b/test/src/osqp_dense_qp_wrapper.cpp @@ -0,0 +1,7677 @@ +// +// Copyright (c) 2022-2024 INRIA +// +#include +#include +#include +#include +#include +#include +#include + +using T = double; +using namespace proxsuite; +using namespace proxsuite::proxqp; +namespace pod = proxsuite::osqp::dense; + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with inequality constraints" + "and empty equality constraints") +{ + std::cout << "---testing sparse random strongly convex qp with inequality " + "constraints " + "and empty equality constraints---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(0); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + + // Testing with empty but properly sized matrix A of size (0, 10) + std::cout << "Solving QP with" << std::endl; + std::cout << "dim: " << dim << std::endl; + std::cout << "n_eq: " << n_eq << std::endl; + std::cout << "n_in: " << n_in << std::endl; + std::cout << "H : " << qp_random.H << std::endl; + std::cout << "g : " << qp_random.g << std::endl; + std::cout << "A : " << qp_random.A << std::endl; + std::cout << "A.cols() : " << qp_random.A.cols() << std::endl; + std::cout << "A.rows() : " << qp_random.A.rows() << std::endl; + std::cout << "b : " << qp_random.b << std::endl; + std::cout << "C : " << qp_random.C << std::endl; + std::cout << "u : " << qp_random.u << std::endl; + std::cout << "l : " << qp_random.l << std::endl; + + qp.init(qp_random.H, + qp_random.g, + nullopt, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm(); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + // Testing with empty matrix A of size (0, 0) + qp_random.A = Eigen::MatrixXd(); + qp_random.b = Eigen::VectorXd(); + + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + + std::cout << "Solving QP with" << std::endl; + std::cout << "dim: " << dim << std::endl; + std::cout << "n_eq: " << n_eq << std::endl; + std::cout << "n_in: " << n_in << std::endl; + std::cout << "H : " << qp_random.H << std::endl; + std::cout << "g : " << qp_random.g << std::endl; + std::cout << "A : " << qp_random.A << std::endl; + std::cout << "A.cols() : " << qp_random.A.cols() << std::endl; + std::cout << "A.rows() : " << qp_random.A.rows() << std::endl; + std::cout << "b : " << qp_random.b << std::endl; + std::cout << "C : " << qp_random.C << std::endl; + std::cout << "u : " << qp_random.u << std::endl; + std::cout << "l : " << qp_random.l << std::endl; + + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.solve(); + + pri_res = (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm(); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + // Testing with nullopt + pod::QP qp3{ dim, n_eq, n_in }; // creating QP object + qp3.settings.eps_abs = eps_abs; + qp3.settings.eps_rel = 0; + + qp3.init(qp_random.H, + qp_random.g, + nullopt, + nullopt, + qp_random.C, + qp_random.l, + qp_random.u); + qp3.solve(); + + pri_res = (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm(); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; +} +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test update H") +{ + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test update H---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "before upating" << std::endl; + std::cout << "H : " << qp_random.H << std::endl; + std::cout << "g : " << qp_random.g << std::endl; + std::cout << "A : " << qp_random.A << std::endl; + std::cout << "b : " << qp_random.b << std::endl; + std::cout << "C : " << qp_random.C << std::endl; + std::cout << "u : " << qp_random.u << std::endl; + std::cout << "l : " << qp_random.l << std::endl; + + std::cout << "testing updating H" << std::endl; + qp_random.H.setIdentity(); + qp.update(qp_random.H, nullopt, nullopt, nullopt, nullopt, nullopt, nullopt); + std::cout << "after upating" << std::endl; + std::cout << "H : " << qp.model.H << std::endl; + std::cout << "g : " << qp.model.g << std::endl; + std::cout << "A : " << qp.model.A << std::endl; + std::cout << "b : " << qp.model.b << std::endl; + std::cout << "C : " << qp.model.C << std::endl; + std::cout << "u : " << qp.model.u << std::endl; + std::cout << "l : " << qp.model.l << std::endl; + + qp.solve(); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim after updating: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.solve(); + + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------ conter factual check with another QP object starting at " + "the updated model : " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test update A") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test update A---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + isize dim = 10; + + isize n_eq(dim / 4); + isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "before upating" << std::endl; + std::cout << "H : " << qp_random.H << std::endl; + std::cout << "g : " << qp_random.g << std::endl; + std::cout << "A : " << qp_random.A << std::endl; + std::cout << "b : " << qp_random.b << std::endl; + std::cout << "C : " << qp_random.C << std::endl; + std::cout << "u : " << qp_random.u << std::endl; + std::cout << "l : " << qp_random.l << std::endl; + + std::cout << "testing updating A" << std::endl; + qp_random.A = utils::rand::sparse_matrix_rand_not_compressed( + n_eq, dim, sparsity_factor); + qp.update(nullopt, nullopt, qp_random.A, nullopt, nullopt, nullopt, nullopt); + + std::cout << "after upating" << std::endl; + std::cout << "H : " << qp.model.H << std::endl; + std::cout << "g : " << qp.model.g << std::endl; + std::cout << "A : " << qp.model.A << std::endl; + std::cout << "b : " << qp.model.b << std::endl; + std::cout << "C : " << qp.model.C << std::endl; + std::cout << "u : " << qp.model.u << std::endl; + std::cout << "l : " << qp.model.l << std::endl; + + qp.solve(); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim after updating: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.solve(); + + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------ conter factual check with another QP object starting at " + "the updated model : " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test update C") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test update C---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + isize dim = 10; + + isize n_eq(dim / 4); + isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "before upating" << std::endl; + std::cout << "H : " << qp_random.H << std::endl; + std::cout << "g : " << qp_random.g << std::endl; + std::cout << "A : " << qp_random.A << std::endl; + std::cout << "b : " << qp_random.b << std::endl; + std::cout << "C : " << qp_random.C << std::endl; + std::cout << "u : " << qp_random.u << std::endl; + std::cout << "l : " << qp_random.l << std::endl; + + std::cout << "testing updating C" << std::endl; + qp_random.C = utils::rand::sparse_matrix_rand_not_compressed( + n_in, dim, sparsity_factor); + qp.update(nullopt, nullopt, nullopt, nullopt, qp_random.C, nullopt, nullopt); + + std::cout << "after upating" << std::endl; + std::cout << "H : " << qp.model.H << std::endl; + std::cout << "g : " << qp.model.g << std::endl; + std::cout << "A : " << qp.model.A << std::endl; + std::cout << "b : " << qp.model.b << std::endl; + std::cout << "C : " << qp.model.C << std::endl; + std::cout << "u : " << qp.model.u << std::endl; + std::cout << "l : " << qp.model.l << std::endl; + + qp.solve(); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim after updating: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.solve(); + + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------ conter factual check with another QP object starting at " + "the updated model : " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test update b") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test update b---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + isize dim = 10; + + isize n_eq(dim / 4); + isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "before upating" << std::endl; + std::cout << "H : " << qp_random.H << std::endl; + std::cout << "g : " << qp_random.g << std::endl; + std::cout << "A : " << qp_random.A << std::endl; + std::cout << "b : " << qp_random.b << std::endl; + std::cout << "C : " << qp_random.C << std::endl; + std::cout << "u : " << qp_random.u << std::endl; + std::cout << "l : " << qp_random.l << std::endl; + + std::cout << "testing updating b" << std::endl; + auto x_sol = utils::rand::vector_rand(dim); + qp_random.b = qp_random.A * x_sol; + qp.update(nullopt, nullopt, nullopt, qp_random.b, nullopt, nullopt, nullopt); + + std::cout << "after upating" << std::endl; + std::cout << "H : " << qp.model.H << std::endl; + std::cout << "g : " << qp.model.g << std::endl; + std::cout << "A : " << qp.model.A << std::endl; + std::cout << "b : " << qp.model.b << std::endl; + std::cout << "C : " << qp.model.C << std::endl; + std::cout << "u : " << qp.model.u << std::endl; + std::cout << "l : " << qp.model.l << std::endl; + + qp.solve(); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim after updating: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.solve(); + + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------ conter factual check with another QP object starting at " + "the updated model : " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test update u") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test update u---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + isize dim = 10; + isize n_eq(dim / 4); + isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "before upating" << std::endl; + std::cout << "H : " << qp_random.H << std::endl; + std::cout << "g : " << qp_random.g << std::endl; + std::cout << "A : " << qp_random.A << std::endl; + std::cout << "b : " << qp_random.b << std::endl; + std::cout << "C : " << qp_random.C << std::endl; + std::cout << "u : " << qp_random.u << std::endl; + std::cout << "l : " << qp_random.l << std::endl; + + std::cout << "testing updating b" << std::endl; + auto x_sol = utils::rand::vector_rand(dim); + auto delta = utils::Vec(n_in); + for (isize i = 0; i < n_in; ++i) { + delta(i) = utils::rand::uniform_rand(); + } + + qp_random.u = qp_random.C * x_sol + delta; + qp.update(nullopt, nullopt, nullopt, nullopt, nullopt, nullopt, qp_random.u); + + std::cout << "after upating" << std::endl; + std::cout << "H : " << qp.model.H << std::endl; + std::cout << "g : " << qp.model.g << std::endl; + std::cout << "A : " << qp.model.A << std::endl; + std::cout << "b : " << qp.model.b << std::endl; + std::cout << "C : " << qp.model.C << std::endl; + std::cout << "u : " << qp.model.u << std::endl; + std::cout << "l : " << qp.model.l << std::endl; + + qp.solve(); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim after updating: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.solve(); + + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------ conter factual check with another QP object starting at " + "the updated model : " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test update g") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test update g---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + isize dim = 10; + isize n_eq(dim / 4); + isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "before upating" << std::endl; + std::cout << "H : " << qp_random.H << std::endl; + std::cout << "g : " << qp_random.g << std::endl; + std::cout << "A : " << qp_random.A << std::endl; + std::cout << "b : " << qp_random.b << std::endl; + std::cout << "C : " << qp_random.C << std::endl; + std::cout << "u : " << qp_random.u << std::endl; + std::cout << "l : " << qp_random.l << std::endl; + + std::cout << "testing updating g" << std::endl; + auto g = utils::rand::vector_rand(dim); + + qp_random.g = g; + qp.update(nullopt, qp_random.g, nullopt, nullopt, nullopt, nullopt, nullopt); + + std::cout << "after upating" << std::endl; + std::cout << "H : " << qp.model.H << std::endl; + std::cout << "g : " << qp.model.g << std::endl; + std::cout << "A : " << qp.model.A << std::endl; + std::cout << "b : " << qp.model.b << std::endl; + std::cout << "C : " << qp.model.C << std::endl; + std::cout << "u : " << qp.model.u << std::endl; + std::cout << "l : " << qp.model.l << std::endl; + + qp.solve(); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim after updating: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.solve(); + + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------ conter factual check with another QP object starting at " + "the updated model : " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "sparse random strongly convex qp with equality and inequality " + "constraints: test update H and A and b and u and l") +{ + + std::cout + << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test update H and A and b and u and l---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + isize dim = 10; + isize n_eq(dim / 4); + isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "before upating" << std::endl; + std::cout << "H : " << qp_random.H << std::endl; + std::cout << "g : " << qp_random.g << std::endl; + std::cout << "A : " << qp_random.A << std::endl; + std::cout << "b : " << qp_random.b << std::endl; + std::cout << "C : " << qp_random.C << std::endl; + std::cout << "u : " << qp_random.u << std::endl; + std::cout << "l : " << qp_random.l << std::endl; + + std::cout << "testing updating b" << std::endl; + qp_random.H = utils::rand::sparse_positive_definite_rand_not_compressed( + dim, strong_convexity_factor, sparsity_factor); + qp_random.A = utils::rand::sparse_matrix_rand_not_compressed( + n_eq, dim, sparsity_factor); + auto x_sol = utils::rand::vector_rand(dim); + auto delta = utils::Vec(n_in); + for (proxqp::isize i = 0; i < n_in; ++i) { + delta(i) = utils::rand::uniform_rand(); + } + qp_random.b = qp_random.A * x_sol; + qp_random.u = qp_random.C * x_sol + delta; + qp_random.l = qp_random.C * x_sol - delta; + qp.update(qp_random.H, + nullopt, + qp_random.A, + qp_random.b, + nullopt, + qp_random.l, + qp_random.u); + + std::cout << "after upating" << std::endl; + std::cout << "H : " << qp.model.H << std::endl; + std::cout << "g : " << qp.model.g << std::endl; + std::cout << "A : " << qp.model.A << std::endl; + std::cout << "b : " << qp.model.b << std::endl; + std::cout << "C : " << qp.model.C << std::endl; + std::cout << "u : " << qp.model.u << std::endl; + std::cout << "l : " << qp.model.l << std::endl; + + qp.solve(); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim after updating: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.solve(); + + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------ conter factual check with another QP object starting at " + "the updated model : " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test update rho") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test update rho---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + isize dim = 10; + isize n_eq(dim / 4); + isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "before upating" << std::endl; + std::cout << "rho : " << qp.results.info.rho << std::endl; + + qp.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + true, + T(1.e-7), + nullopt, + nullopt); // restart the problem with default options + std::cout << "after upating" << std::endl; + std::cout << "rho : " << qp.results.info.rho << std::endl; + + qp.solve(); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim after updating: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + T(1.e-7), + nullopt, + nullopt); + std::cout << "rho : " << qp2.results.info.rho << std::endl; + qp2.solve(); + + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------ conter factual check with another QP object starting at " + "the updated model : " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test update mu_eq and mu_in") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test update mu_eq and mu_in---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + isize dim = 10; + isize n_eq(dim / 4); + isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "before upating" << std::endl; + std::cout << "mu_in : " << qp.results.info.mu_in << std::endl; + std::cout << "mu_eq : " << qp.results.info.mu_eq << std::endl; + + qp.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + true, + nullopt, + T(1.e-2), + T(1.e-3)); + + std::cout << "after upating" << std::endl; + std::cout << "mu_in : " << qp.results.info.mu_in << std::endl; + std::cout << "mu_eq : " << qp.results.info.mu_eq << std::endl; + + qp.solve(); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim after updating: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + nullopt, + T(1.e-2), + T(1.e-3)); + qp2.solve(); + std::cout << "mu_in : " << qp2.results.info.mu_in << std::endl; + std::cout << "mu_eq : " << qp2.results.info.mu_eq << std::endl; + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------ conter factual check with another QP object starting at " + "the updated model : " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test warm starting") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test warm starting---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + isize dim = 10; + isize n_eq(dim / 4); + isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + auto x_wm = utils::rand::vector_rand(dim); + auto y_wm = utils::rand::vector_rand(n_eq); + auto z_wm = utils::rand::vector_rand(n_in); + std::cout << "proposed warm start" << std::endl; + std::cout << "x_wm : " << x_wm << std::endl; + std::cout << "y_wm : " << y_wm << std::endl; + std::cout << "z_wm : " << z_wm << std::endl; + qp.settings.initial_guess = InitialGuessStatus::WARM_START; + qp.solve(x_wm, y_wm, z_wm); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------using API solving qp with dim after updating: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.settings.initial_guess = InitialGuessStatus::WARM_START; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.solve(x_wm, y_wm, z_wm); + + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------ conter factual check with another QP object starting at " + "the updated model : " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test dense init") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test dense init---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init( + Eigen::Matrix( + qp_random.H), + qp_random.g, + Eigen::Matrix( + qp_random.A), + qp_random.b, + Eigen::Matrix( + qp_random.C), + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test with no initial guess") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test with no initial guess---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + std::cout << "------using API solving qp with dim with qp: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.solve(); + + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + std::cout << "------using API solving qp with dim with qp2: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "sparse random strongly convex qp with equality and " + "inequality constraints: test with equality constrained initial guess") +{ + + std::cout + << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test with equality constrained initial guess---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + std::cout << "------using API solving qp with dim with qp: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.solve(); + + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + std::cout << "------using API solving qp with dim with qp2: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "sparse random strongly convex qp with equality and " + "inequality constraints: test with warm start with previous result") +{ + + std::cout + << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test with warm start with previous result---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + std::cout << "------using API solving qp with dim with qp: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.settings.initial_guess = InitialGuessStatus::WARM_START; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true); + + auto x = qp.results.x; + auto y = qp.results.y; + auto z = qp.results.z; + // std::cout << "after scaling x " << x << " qp.results.x " << qp.results.x + // << std::endl; + qp2.ruiz.scale_primal_in_place({ from_eigen, x }); + qp2.ruiz.scale_dual_in_place_eq({ from_eigen, y }); + qp2.ruiz.scale_dual_in_place_in({ from_eigen, z }); + // std::cout << "after scaling x " << x << " qp.results.x " << qp.results.x + // << std::endl; + qp2.solve(x, y, z); + + qp.settings.initial_guess = + InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + qp.update( + nullopt, nullopt, nullopt, nullopt, nullopt, nullopt, nullopt, false); + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + std::cout << "------using API solving qp with dim with qp after warm start " + "with previous result: " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + std::cout << "------using API solving qp with dim with qp2: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test with cold start option") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test with cold start option---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + std::cout << "------using API solving qp with dim with qp: " << dim + << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.settings.initial_guess = InitialGuessStatus::WARM_START; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true); + + auto x = qp.results.x; + auto y = qp.results.y; + auto z = qp.results.z; + // std::cout << "after scaling x " << x << " qp.results.x " << qp.results.x + // << std::endl; + qp2.ruiz.scale_primal_in_place({ from_eigen, x }); + qp2.ruiz.scale_dual_in_place_eq({ from_eigen, y }); + qp2.ruiz.scale_dual_in_place_in({ from_eigen, z }); + // std::cout << "after scaling x " << x << " qp.results.x " << qp.results.x + // << std::endl; + qp2.solve(x, y, z); + + qp.settings.initial_guess = + InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT; + qp.update( + nullopt, nullopt, nullopt, nullopt, nullopt, nullopt, nullopt, true); + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + std::cout << "------using API solving qp with dim with qp after warm start " + "with cold start option: " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + std::cout << "------using API solving qp with dim with cold start option: " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "sparse random strongly convex qp with equality and " + "inequality constraints: test equilibration options at initialization") +{ + + std::cout + << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test equilibration options at initialization---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + std::cout << "------using API solving qp with dim with qp with " + "preconditioner derived: " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "ruiz vector : " << qp.ruiz.delta << " ruiz scalar factor " + << qp.ruiz.c << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + false); + qp2.solve(); + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + std::cout << "------using API solving qp without preconditioner derivation: " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "ruiz vector : " << qp2.ruiz.delta << " ruiz scalar factor " + << qp2.ruiz.c << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "sparse random strongly convex qp with equality and " + "inequality constraints: test equilibration options at update") +{ + + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test equilibration options at update---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true); + qp.solve(); + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + std::cout << "------using API solving qp with dim with qp with " + "preconditioner derived: " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + qp.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + true); // rederive preconditioner with previous options, i.e., redo + // exact same derivations + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + std::cout << "------using API solving qp with preconditioner re derived " + "after an update (should get exact same results): " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true); + qp2.solve(); + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + std::cout << "------using API solving qp with preconditioner derivation and " + "another object QP: " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; + + qp2.update( + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + false); // use previous preconditioner: should get same result as well + qp2.solve(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + std::cout << "------using API solving qp without preconditioner derivation: " + << dim << " neq: " << n_eq << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "ruiz vector : " << qp2.ruiz.delta << " ruiz scalar factor " + << qp2.ruiz.c << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +///// TESTS ALL INITIAL GUESS OPTIONS FOR MULTIPLE SOLVES AT ONCE +TEST_CASE( + "sparse random strongly convex qp with equality and " + "inequality constraints: test multiple solve at once with no initial guess") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + + std::cout << "Test with no initial guess" << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Second solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Third solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Fourth solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; +} + +TEST_CASE("ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test multiple solve at once with equality " + "constrained initial guess") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + + std::cout << "Test with equality constrained initial guess" << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Second solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Third solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Fourth solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; +} + +TEST_CASE("ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test multiple solve at once with equality " + "constrained initial guess") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + + std::cout << "Test with warm start with previous result and first solve with " + "equality constrained initial guess" + << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + qp.settings.initial_guess = + InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Second solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Third solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Fourth solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; +} + +TEST_CASE( + "sparse random strongly convex qp with equality and " + "inequality constraints: test multiple solve at once with no initial guess") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + + std::cout << "Test with warm start with previous result and first solve with " + "no initial guess" + << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + qp.settings.initial_guess = + InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Second solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Third solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Fourth solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; +} + +TEST_CASE("ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test multiple solve at once with cold start " + "initial guess") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + + std::cout << "Test with cold start with previous result and first solve with " + "equality constrained initial guess" + << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + qp.settings.initial_guess = + InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT; + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Second solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Third solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Fourth solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; +} + +TEST_CASE("ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test multiple solve at once with warm start") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + + std::cout << "Test with warm start and first solve with no initial guess" + << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + qp.settings.initial_guess = InitialGuessStatus::WARM_START; + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(qp.results.x, qp.results.y, qp.results.z); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Second solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(qp.results.x, qp.results.y, qp.results.z); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Third solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(qp.results.x, qp.results.y, qp.results.z); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Fourth solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; +} + +TEST_CASE("ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: warm start test from init") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + + std::cout << "Test with warm start and first solve with no initial guess" + << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + pod::QP qp2(dim, n_eq, n_in); + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp2.settings.initial_guess = InitialGuessStatus::WARM_START; + std::cout << "dirty workspace for qp2 : " << qp2.work.dirty << std::endl; + qp2.solve(qp.results.x, qp.results.y, qp.results.z); + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Second solve with new QP object" << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +/// TESTS WITH UPDATE + INITIAL GUESS OPTIONS + +TEST_CASE("ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test update and multiple solve at once with " + "no initial guess") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + + std::cout << "Test with no initial guess" << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp_random.H *= 2.; + qp_random.g = utils::rand::vector_rand(dim); + bool update_preconditioner = true; + qp.update(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + update_preconditioner); + std::cout << "dirty workspace after update : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Second solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Third solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Fourth solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; +} + +TEST_CASE("ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test update + multiple solve at once with " + "equality constrained initial guess") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + + std::cout << "Test with equality constrained initial guess" << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp_random.H *= 2.; + qp_random.g = utils::rand::vector_rand(dim); + bool update_preconditioner = true; + qp.update(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + update_preconditioner); + std::cout << "dirty workspace after update : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Second solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Third solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Fourth solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; +} + +TEST_CASE( + "sparse random strongly convex qp with equality and " + "inequality constraints: test update + multiple solve at once with equality " + "constrained initial guess and then warm start with previous results") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + + std::cout << "Test with warm start with previous result and first solve with " + "equality constrained initial guess" + << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + qp.settings.initial_guess = + InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp_random.H *= 2.; + qp_random.g = utils::rand::vector_rand(dim); + bool update_preconditioner = true; + qp.update(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + update_preconditioner); + std::cout << "dirty workspace after update : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Second solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Third solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Fourth solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; +} + +TEST_CASE( + "sparse random strongly convex qp with equality and " + "inequality constraints: test multiple solve at once with no initial guess") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + + std::cout << "Test with warm start with previous result and first solve with " + "no initial guess" + << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + qp.settings.initial_guess = + InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp_random.H *= 2.; + qp_random.g = utils::rand::vector_rand(dim); + bool update_preconditioner = true; + qp.update(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + update_preconditioner); + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Second solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Third solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Fourth solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; +} + +TEST_CASE("ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test update + multiple solve at once with " + "cold start initial guess and then cold start option") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + + std::cout << "Test with cold start with previous result and first solve with " + "equality constrained initial guess" + << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + qp.settings.initial_guess = + InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT; + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp_random.H *= 2.; + qp_random.g = utils::rand::vector_rand(dim); + bool update_preconditioner = true; + qp.update(qp_random.H, + qp_random.g, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + update_preconditioner); + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Second solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Third solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Fourth solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; +} + +TEST_CASE("ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test update + multiple solve at once with " + "warm start") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + + std::cout << "Test with warm start and first solve with no initial guess" + << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + qp.settings.initial_guess = InitialGuessStatus::WARM_START; + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + auto x_wm = qp.results.x; // keep previous result + auto y_wm = qp.results.y; + auto z_wm = qp.results.z; + bool update_preconditioner = true; + // test with a false update (the warm start should give the exact solution) + qp.update(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + update_preconditioner); + std::cout << "dirty workspace after update: " << qp.work.dirty << std::endl; + qp.solve(x_wm, y_wm, z_wm); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + std::cout << "Second solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + x_wm = qp.results.x; // keep previous result + y_wm = qp.results.y; + z_wm = qp.results.z; + qp_random.H *= 2.; + qp_random.g = utils::rand::vector_rand(dim); + // try now with a real update + qp.update(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + update_preconditioner); + std::cout << "dirty workspace after update: " << qp.work.dirty << std::endl; + qp.solve(x_wm, y_wm, z_wm); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Third solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(qp.results.x, qp.results.y, qp.results.z); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Fourth solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + std::cout << "dirty workspace : " << qp.work.dirty << std::endl; + qp.solve(qp.results.x, qp.results.y, qp.results.z); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "Fifth solve " << std::endl; + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; +} + +TEST_CASE( + "ProxQP::dense: Test initializaton with rho for different initial guess") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + + std::cout << "Test initializaton with rho for different initial guess" + << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + T(1.E-7)); + qp.solve(); + CHECK(qp.results.info.rho == T(1.E-7)); + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + pod::QP qp2(dim, n_eq, n_in); + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.settings.initial_guess = + InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + T(1.E-7)); + qp2.solve(); + CHECK(qp2.results.info.rho == T(1.E-7)); + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; + + pod::QP qp3(dim, n_eq, n_in); + qp3.settings.eps_abs = eps_abs; + qp3.settings.eps_rel = 0; + qp3.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + qp3.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + T(1.E-7)); + qp3.solve(); + CHECK(qp3.results.info.rho == T(1.E-7)); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp3.results.info.iter + << std::endl; + std::cout << "setup timing " << qp3.results.info.setup_time << " solve time " + << qp3.results.info.solve_time << std::endl; + + pod::QP qp4(dim, n_eq, n_in); + qp4.settings.eps_abs = eps_abs; + qp4.settings.eps_rel = 0; + qp4.settings.initial_guess = + InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT; + qp4.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + T(1.E-7)); + qp4.solve(); + CHECK(qp4.results.info.rho == T(1.E-7)); + pri_res = std::max( + (qp_random.A * qp4.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp4.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp4.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp4.results.x + qp_random.g + + qp_random.A.transpose() * qp4.results.y + + qp_random.C.transpose() * qp4.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp4.results.info.iter + << std::endl; + std::cout << "setup timing " << qp4.results.info.setup_time << " solve time " + << qp4.results.info.solve_time << std::endl; + + pod::QP qp5(dim, n_eq, n_in); + qp5.settings.eps_abs = eps_abs; + qp5.settings.eps_rel = 0; + qp5.settings.initial_guess = InitialGuessStatus::WARM_START; + qp5.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + T(1.E-7)); + qp5.solve(qp3.results.x, qp3.results.y, qp3.results.z); + CHECK(qp5.results.info.rho == T(1.E-7)); + pri_res = std::max( + (qp_random.A * qp5.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp5.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp5.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp5.results.x + qp_random.g + + qp_random.A.transpose() * qp5.results.y + + qp_random.C.transpose() * qp5.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp5.results.info.iter + << std::endl; + std::cout << "setup timing " << qp5.results.info.setup_time << " solve time " + << qp5.results.info.solve_time << std::endl; +} + +TEST_CASE("ProxQP::dense: Test g update for different initial guess") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + + std::cout << "Test g update for different initial guess" << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + auto old_g = qp_random.g; + qp_random.g = utils::rand::vector_rand(dim); + qp.update(nullopt, qp_random.g, nullopt, nullopt, nullopt, nullopt, nullopt); + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK((qp.model.g - qp_random.g).lpNorm() <= eps_abs); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + pod::QP qp2(dim, n_eq, n_in); + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.settings.initial_guess = + InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + qp2.init(qp_random.H, + old_g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.solve(); + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + old_g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + qp2.update(nullopt, qp_random.g, nullopt, nullopt, nullopt, nullopt, nullopt); + qp2.solve(); + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + CHECK((qp2.model.g - qp_random.g).lpNorm() <= eps_abs); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; + + pod::QP qp3(dim, n_eq, n_in); + qp3.settings.eps_abs = eps_abs; + qp3.settings.eps_rel = 0; + qp3.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + qp3.init(qp_random.H, + old_g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp3.solve(); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + old_g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + qp3.update(nullopt, qp_random.g, nullopt, nullopt, nullopt, nullopt, nullopt); + qp3.solve(); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + CHECK((qp3.model.g - qp_random.g).lpNorm() <= eps_abs); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp3.results.info.iter + << std::endl; + std::cout << "setup timing " << qp3.results.info.setup_time << " solve time " + << qp3.results.info.solve_time << std::endl; + + pod::QP qp4(dim, n_eq, n_in); + qp4.settings.eps_abs = eps_abs; + qp4.settings.eps_rel = 0; + qp4.settings.initial_guess = + InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT; + qp4.init(qp_random.H, + old_g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp4.solve(); + pri_res = std::max( + (qp_random.A * qp4.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp4.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp4.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp4.results.x + old_g + + qp_random.A.transpose() * qp4.results.y + + qp_random.C.transpose() * qp4.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + qp4.update(nullopt, qp_random.g, nullopt, nullopt, nullopt, nullopt, nullopt); + qp4.solve(); + pri_res = std::max( + (qp_random.A * qp4.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp4.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp4.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp4.results.x + qp_random.g + + qp_random.A.transpose() * qp4.results.y + + qp_random.C.transpose() * qp4.results.z) + .lpNorm(); + CHECK((qp4.model.g - qp_random.g).lpNorm() <= eps_abs); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp4.results.info.iter + << std::endl; + std::cout << "setup timing " << qp4.results.info.setup_time << " solve time " + << qp4.results.info.solve_time << std::endl; + + pod::QP qp5(dim, n_eq, n_in); + qp5.settings.eps_abs = eps_abs; + qp5.settings.eps_rel = 0; + qp5.settings.initial_guess = InitialGuessStatus::WARM_START; + qp5.init(qp_random.H, + old_g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp5.solve(qp3.results.x, qp3.results.y, qp3.results.z); + pri_res = std::max( + (qp_random.A * qp5.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp5.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp5.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp5.results.x + old_g + + qp_random.A.transpose() * qp5.results.y + + qp_random.C.transpose() * qp5.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + qp5.update(nullopt, qp_random.g, nullopt, nullopt, nullopt, nullopt, nullopt); + qp5.solve(); + pri_res = std::max( + (qp_random.A * qp5.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp5.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp5.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp5.results.x + qp_random.g + + qp_random.A.transpose() * qp5.results.y + + qp_random.C.transpose() * qp5.results.z) + .lpNorm(); + CHECK((qp5.model.g - qp_random.g).lpNorm() <= eps_abs); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp5.results.info.iter + << std::endl; + std::cout << "setup timing " << qp5.results.info.setup_time << " solve time " + << qp5.results.info.solve_time << std::endl; +} + +TEST_CASE("ProxQP::dense: Test A update for different initial guess") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + + std::cout << "Test A update for different initial guess" << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + auto new_A = utils::rand::sparse_matrix_rand_not_compressed( + n_eq, dim, sparsity_factor); + qp.update(nullopt, nullopt, new_A, nullopt, nullopt, nullopt, nullopt); + qp.solve(); + pri_res = + std::max((new_A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = + (qp_random.H * qp.results.x + qp_random.g + + new_A.transpose() * qp.results.y + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK((qp.model.A - new_A).lpNorm() <= eps_abs); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + pod::QP qp2(dim, n_eq, n_in); + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.settings.initial_guess = + InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.solve(); + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + qp2.update(nullopt, nullopt, new_A, nullopt, nullopt, nullopt, nullopt); + qp2.solve(); + pri_res = std::max( + (new_A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + new_A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + CHECK((qp2.model.A - new_A).lpNorm() <= eps_abs); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; + + pod::QP qp3(dim, n_eq, n_in); + qp3.settings.eps_abs = eps_abs; + qp3.settings.eps_rel = 0; + qp3.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + qp3.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp3.solve(); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + qp3.update(nullopt, nullopt, new_A, nullopt, nullopt, nullopt, nullopt); + qp3.solve(); + pri_res = std::max( + (new_A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + new_A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + CHECK((qp3.model.A - new_A).lpNorm() <= eps_abs); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp3.results.info.iter + << std::endl; + std::cout << "setup timing " << qp3.results.info.setup_time << " solve time " + << qp3.results.info.solve_time << std::endl; + + pod::QP qp4(dim, n_eq, n_in); + qp4.settings.eps_abs = eps_abs; + qp4.settings.eps_rel = 0; + qp4.settings.initial_guess = + InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT; + qp4.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp4.solve(); + pri_res = std::max( + (qp_random.A * qp4.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp4.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp4.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp4.results.x + qp_random.g + + qp_random.A.transpose() * qp4.results.y + + qp_random.C.transpose() * qp4.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + qp4.update(nullopt, nullopt, new_A, nullopt, nullopt, nullopt, nullopt); + qp4.solve(); + pri_res = std::max( + (new_A * qp4.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp4.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp4.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp4.results.x + qp_random.g + + new_A.transpose() * qp4.results.y + + qp_random.C.transpose() * qp4.results.z) + .lpNorm(); + CHECK((qp4.model.A - new_A).lpNorm() <= eps_abs); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp4.results.info.iter + << std::endl; + std::cout << "setup timing " << qp4.results.info.setup_time << " solve time " + << qp4.results.info.solve_time << std::endl; + + pod::QP qp5(dim, n_eq, n_in); + qp5.settings.eps_abs = eps_abs; + qp5.settings.eps_rel = 0; + qp5.settings.initial_guess = InitialGuessStatus::WARM_START; + qp5.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp5.solve(qp3.results.x, qp3.results.y, qp3.results.z); + pri_res = std::max( + (qp_random.A * qp5.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp5.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp5.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp5.results.x + qp_random.g + + qp_random.A.transpose() * qp5.results.y + + qp_random.C.transpose() * qp5.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + qp5.update(nullopt, nullopt, new_A, nullopt, nullopt, nullopt, nullopt); + qp5.solve(); + pri_res = std::max( + (new_A * qp5.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp5.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp5.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp5.results.x + qp_random.g + + new_A.transpose() * qp5.results.y + + qp_random.C.transpose() * qp5.results.z) + .lpNorm(); + CHECK((qp5.model.A - new_A).lpNorm() <= eps_abs); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp5.results.info.iter + << std::endl; + std::cout << "setup timing " << qp5.results.info.setup_time << " solve time " + << qp5.results.info.solve_time << std::endl; +} + +TEST_CASE("ProxQP::dense: Test rho update for different initial guess") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + + std::cout << "Test rho update for different initial guess" << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + qp.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + true, + T(1.E-7)); + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(qp.results.info.rho == T(1.E-7)); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + pod::QP qp2(dim, n_eq, n_in); + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.settings.initial_guess = + InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.solve(); + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + qp2.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + true, + T(1.E-7)); + qp2.solve(); + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + CHECK(qp2.results.info.rho == T(1.e-7)); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; + + pod::QP qp3(dim, n_eq, n_in); + qp3.settings.eps_abs = eps_abs; + qp3.settings.eps_rel = 0; + qp3.settings.initial_guess = + InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + qp3.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp3.solve(); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + qp3.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + true, + T(1.E-7)); + qp3.solve(); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + CHECK(qp3.results.info.rho == T(1.e-7)); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp3.results.info.iter + << std::endl; + std::cout << "setup timing " << qp3.results.info.setup_time << " solve time " + << qp3.results.info.solve_time << std::endl; + + pod::QP qp4(dim, n_eq, n_in); + qp4.settings.eps_abs = eps_abs; + qp4.settings.eps_rel = 0; + qp4.settings.initial_guess = + InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT; + qp4.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp4.solve(); + pri_res = std::max( + (qp_random.A * qp4.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp4.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp4.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp4.results.x + qp_random.g + + qp_random.A.transpose() * qp4.results.y + + qp_random.C.transpose() * qp4.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + qp4.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + true, + T(1.E-7)); + qp4.solve(); + pri_res = std::max( + (qp_random.A * qp4.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp4.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp4.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp4.results.x + qp_random.g + + qp_random.A.transpose() * qp4.results.y + + qp_random.C.transpose() * qp4.results.z) + .lpNorm(); + CHECK(qp4.results.info.rho == T(1.e-7)); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp4.results.info.iter + << std::endl; + std::cout << "setup timing " << qp4.results.info.setup_time << " solve time " + << qp4.results.info.solve_time << std::endl; + + pod::QP qp5(dim, n_eq, n_in); + qp5.settings.eps_abs = eps_abs; + qp5.settings.eps_rel = 0; + qp5.settings.initial_guess = InitialGuessStatus::WARM_START; + qp5.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp5.solve(qp3.results.x, qp3.results.y, qp3.results.z); + pri_res = std::max( + (qp_random.A * qp5.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp5.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp5.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp5.results.x + qp_random.g + + qp_random.A.transpose() * qp5.results.y + + qp_random.C.transpose() * qp5.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + qp5.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + true, + T(1.E-7)); + qp5.solve(); + pri_res = std::max( + (qp_random.A * qp5.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp5.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp5.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp5.results.x + qp_random.g + + qp_random.A.transpose() * qp5.results.y + + qp_random.C.transpose() * qp5.results.z) + .lpNorm(); + CHECK(qp5.results.info.rho == T(1.e-7)); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp5.results.info.iter + << std::endl; + std::cout << "setup timing " << qp5.results.info.setup_time << " solve time " + << qp5.results.info.solve_time << std::endl; +} + +TEST_CASE("ProxQP::dense: Test g update for different warm start with previous " + "result option") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = + InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + + std::cout << "Test rho update for different initial guess" << std::endl; + std::cout << "dirty workspace before any solving: " << qp.work.dirty + << std::endl; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + // a new linear cost slightly modified + auto g = qp_random.g * 0.95; + + qp.update(nullopt, g, nullopt, nullopt, nullopt, nullopt, nullopt); + qp.solve(); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = + (qp_random.H * qp.results.x + g + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter + << std::endl; + std::cout << "setup timing " << qp.results.info.setup_time << " solve time " + << qp.results.info.solve_time << std::endl; + + pod::QP qp2(dim, n_eq, n_in); + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.settings.initial_guess = + InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + qp2.init(qp_random.H, + g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp2.solve(); + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = + (qp_random.H * qp2.results.x + g + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + std::cout << "--n = " << dim << " n_eq " << n_eq << " n_in " << n_in + << std::endl; + std::cout << "; dual residual " << dua_res << "; primal residual " << pri_res + << std::endl; + std::cout << "total number of iteration: " << qp2.results.info.iter + << std::endl; + std::cout << "setup timing " << qp2.results.info.setup_time << " solve time " + << qp2.results.info.solve_time << std::endl; +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings " + "after updates using warm start with previous results") +{ + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings after " + "updates using warm start with previous results---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + T rho(1.e-7); + T mu_eq(1.e-4); + bool compute_preconditioner = true; + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + DOCTEST_CHECK(qp.settings.initial_guess == + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS); + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + qp.solve(); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + qp.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6); + qp.settings.initial_guess = + proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + + DOCTEST_CHECK(std::abs(1.e-6 - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp.results.info.rho) <= 1.E-9); + qp.solve(); + DOCTEST_CHECK(std::abs(1.e-6 - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp.results.info.rho) <= 1.E-9); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + DOCTEST_CHECK(qp2.settings.initial_guess == + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS); + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + nullopt, + mu_eq); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + qp2.solve(); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + // conter factual check with another QP object starting at the updated model + pod::QP qp3{ dim, n_eq, n_in }; // creating QP object + DOCTEST_CHECK(qp3.settings.initial_guess == + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS); + qp3.settings.eps_abs = eps_abs; + qp3.settings.eps_rel = 0; + qp3.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho, + mu_eq); + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + qp3.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6, + 1.e-3); + qp3.settings.initial_guess = + proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + DOCTEST_CHECK(std::abs(1.e-6 - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e3 - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings " + "after updates using cold start with previous results") +{ + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings after " + "updates using cold start with previous results---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + T rho(1.e-7); + T mu_eq(1.e-4); + bool compute_preconditioner = true; + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.initial_guess = + proxqp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT; + DOCTEST_CHECK(qp.settings.initial_guess == + proxqp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT); + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + qp.solve(); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + qp.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6); + DOCTEST_CHECK(std::abs(1.e-6 - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp.results.info.rho) <= 1.E-9); + qp.solve(); + DOCTEST_CHECK(std::abs(1.e-6 - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp.results.info.rho) <= 1.E-9); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.initial_guess = + proxqp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT; + DOCTEST_CHECK(qp2.settings.initial_guess == + proxqp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT); + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + nullopt, + mu_eq); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + qp2.solve(); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + // conter factual check with another QP object starting at the updated model + pod::QP qp3{ dim, n_eq, n_in }; // creating QP object + qp3.settings.initial_guess = + proxqp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT; + DOCTEST_CHECK(qp3.settings.initial_guess == + proxqp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT); + qp3.settings.eps_abs = eps_abs; + qp3.settings.eps_rel = 0; + qp3.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho, + mu_eq); + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + qp3.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6, + 1.e-3); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e3 - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings " + "after updates using equality constrained initial guess") +{ + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings after " + "updates using equality constrained initial guess---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + T rho(1.e-7); + T mu_eq(1.e-4); + bool compute_preconditioner = true; + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.initial_guess = + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + DOCTEST_CHECK(qp.settings.initial_guess == + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS); + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + qp.solve(); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + qp.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6); + DOCTEST_CHECK(std::abs(1.e-6 - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp.results.info.rho) <= 1.E-9); + qp.solve(); + DOCTEST_CHECK(std::abs(1.e-6 - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp.results.info.rho) <= 1.E-9); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.initial_guess = + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + DOCTEST_CHECK(qp2.settings.initial_guess == + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS); + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + nullopt, + mu_eq); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + qp2.solve(); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + // conter factual check with another QP object starting at the updated model + pod::QP qp3{ dim, n_eq, n_in }; // creating QP object + qp3.settings.initial_guess = + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + DOCTEST_CHECK(qp3.settings.initial_guess == + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS); + qp3.settings.eps_abs = eps_abs; + qp3.settings.eps_rel = 0; + qp3.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho, + mu_eq); + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + qp3.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6, + 1.e-3); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e3 - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings " + "after updates using no initial guess") +{ + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings after " + "updates using no initial guess---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + T rho(1.e-7); + T mu_eq(1.e-4); + bool compute_preconditioner = true; + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.initial_guess = proxqp::InitialGuessStatus::NO_INITIAL_GUESS; + DOCTEST_CHECK(qp.settings.initial_guess == + proxqp::InitialGuessStatus::NO_INITIAL_GUESS); + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + qp.solve(); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + qp.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6); + DOCTEST_CHECK(std::abs(1.e-6 - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp.results.info.rho) <= 1.E-9); + qp.solve(); + DOCTEST_CHECK(std::abs(1.e-6 - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp.results.info.rho) <= 1.E-9); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.initial_guess = proxqp::InitialGuessStatus::NO_INITIAL_GUESS; + DOCTEST_CHECK(qp2.settings.initial_guess == + proxqp::InitialGuessStatus::NO_INITIAL_GUESS); + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + nullopt, + mu_eq); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + qp2.solve(); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + // conter factual check with another QP object starting at the updated model + pod::QP qp3{ dim, n_eq, n_in }; // creating QP object + qp3.settings.initial_guess = proxqp::InitialGuessStatus::NO_INITIAL_GUESS; + DOCTEST_CHECK(qp3.settings.initial_guess == + proxqp::InitialGuessStatus::NO_INITIAL_GUESS); + qp3.settings.eps_abs = eps_abs; + qp3.settings.eps_rel = 0; + qp3.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho, + mu_eq); + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + qp3.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6, + 1.e-3); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e3 - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings " + "after several solves using warm start with previous results") +{ + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings after " + "several solves using warm start with previous results---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + T rho(1.e-7); + T mu_eq(1.e-4); + bool compute_preconditioner = true; + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + DOCTEST_CHECK(qp.settings.initial_guess == + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS); + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + qp.solve(); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + qp.settings.initial_guess = + proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + for (isize iter = 0; iter < 10; ++iter) { + qp.solve(); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) < 1.e-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) < 1.e-9); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + qp.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6); + for (isize iter = 0; iter < 10; ++iter) { + qp.solve(); + DOCTEST_CHECK(std::abs(1.e-6 - qp.settings.default_rho) < 1.e-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp.results.info.rho) < 1.e-9); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + DOCTEST_CHECK(qp2.settings.initial_guess == + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS); + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + nullopt, + mu_eq); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + qp2.solve(); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + + qp2.settings.initial_guess = + proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + for (isize iter = 0; iter < 10; ++iter) { + // warm start with previous result used, hence if the qp is small and + // simple, the parameters should not changed during first solve, and also + // after as we start at the solution + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + qp2.solve(); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + // conter factual check with another QP object starting at the updated model + pod::QP qp3{ dim, n_eq, n_in }; // creating QP object + DOCTEST_CHECK(qp3.settings.initial_guess == + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS); + qp3.settings.eps_abs = eps_abs; + qp3.settings.eps_rel = 0; + qp3.settings.verbose = true; + qp3.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho, + mu_eq); + + for (isize iter = 0; iter < 10; ++iter) { + // warm start with previous result used, hence if the qp is small and + // simple, the parameters should not changed during first solve, and also + // after as we start at the solution + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + qp3.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6, + 1.e-3); + for (isize iter = 0; iter < 10; ++iter) { + // warm start with previous result used, hence if the qp is small and + // simple, the parameters should not changed during first solve, and also + // after as we start at the solution + DOCTEST_CHECK(std::abs(1.e-6 - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e3 - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e3 - qp3.results.info.mu_eq_inv) <= 1.E-9); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings " + "after several solves using cold start with previous results") +{ + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings after " + "several solves using cold start with previous results---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + T rho(1.e-7); + T mu_eq(1.e-4); + bool compute_preconditioner = true; + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.initial_guess = + proxqp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT; + DOCTEST_CHECK(qp.settings.initial_guess == + proxqp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT); + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + qp.solve(); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + for (isize iter = 0; iter < 10; ++iter) { + qp.solve(); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) < 1.e-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) < 1.e-9); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + qp.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6); + for (isize iter = 0; iter < 10; ++iter) { + qp.solve(); + DOCTEST_CHECK(std::abs(1.e-6 - qp.settings.default_rho) < 1.e-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp.results.info.rho) < 1.e-9); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.initial_guess = + proxqp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT; + DOCTEST_CHECK(qp2.settings.initial_guess == + proxqp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT); + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + nullopt, + mu_eq); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + qp2.solve(); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + + for (isize iter = 0; iter < 10; ++iter) { + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + qp2.solve(); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + // conter factual check with another QP object starting at the updated model + pod::QP qp3{ dim, n_eq, n_in }; // creating QP object + qp3.settings.initial_guess = + proxqp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT; + DOCTEST_CHECK(qp3.settings.initial_guess == + proxqp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT); + qp3.settings.eps_abs = eps_abs; + qp3.settings.eps_rel = 0; + qp3.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho, + mu_eq); + + for (isize iter = 0; iter < 10; ++iter) { + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + qp3.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6, + 1.e-3); + for (isize iter = 0; iter < 10; ++iter) { + DOCTEST_CHECK(std::abs(1.e-6 - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e3 - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e3 - qp3.results.info.mu_eq_inv) <= 1.E-9); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } +} + +DOCTEST_TEST_CASE( + "sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings after several solves " + "using equality constrained initial guess") +{ + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings after " + "several solves using equality constrained initial guess---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + T rho(1.e-7); + T mu_eq(1.e-4); + bool compute_preconditioner = true; + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.initial_guess = + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + DOCTEST_CHECK(qp.settings.initial_guess == + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS); + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + qp.solve(); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + for (isize iter = 0; iter < 10; ++iter) { + qp.solve(); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) < 1.e-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) < 1.e-9); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + qp.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6); + for (isize iter = 0; iter < 10; ++iter) { + qp.solve(); + DOCTEST_CHECK(std::abs(1.e-6 - qp.settings.default_rho) < 1.e-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp.results.info.rho) < 1.e-9); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.initial_guess = + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + DOCTEST_CHECK(qp2.settings.initial_guess == + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS); + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + nullopt, + mu_eq); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + qp2.solve(); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + + for (isize iter = 0; iter < 10; ++iter) { + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + qp2.solve(); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + // conter factual check with another QP object starting at the updated model + pod::QP qp3{ dim, n_eq, n_in }; // creating QP object + qp3.settings.initial_guess = + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + DOCTEST_CHECK(qp3.settings.initial_guess == + proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS); + qp3.settings.eps_abs = eps_abs; + qp3.settings.eps_rel = 0; + qp3.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho, + mu_eq); + + for (isize iter = 0; iter < 10; ++iter) { + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + qp3.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6, + 1.e-3); + for (isize iter = 0; iter < 10; ++iter) { + DOCTEST_CHECK(std::abs(1.e-6 - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e3 - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e3 - qp3.results.info.mu_eq_inv) <= 1.E-9); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } +} + +DOCTEST_TEST_CASE( + "ProxQP::dense: sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings " + "after several solves using no initial guess") +{ + std::cout << "---testing sparse random strongly convex qp with equality and " + "inequality constraints: test changing default settings after " + "several solves using no initial guess---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + T rho(1.e-7); + T mu_eq(1.e-4); + bool compute_preconditioner = true; + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.initial_guess = proxqp::InitialGuessStatus::NO_INITIAL_GUESS; + DOCTEST_CHECK(qp.settings.initial_guess == + proxqp::InitialGuessStatus::NO_INITIAL_GUESS); + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + qp.solve(); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) <= 1.E-9); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + for (isize iter = 0; iter < 10; ++iter) { + qp.solve(); + DOCTEST_CHECK(std::abs(rho - qp.settings.default_rho) < 1.e-9); + DOCTEST_CHECK(std::abs(rho - qp.results.info.rho) < 1.e-9); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + qp.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6); + for (isize iter = 0; iter < 10; ++iter) { + qp.solve(); + DOCTEST_CHECK(std::abs(1.e-6 - qp.settings.default_rho) < 1.e-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp.results.info.rho) < 1.e-9); + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + // conter factual check with another QP object starting at the updated model + pod::QP qp2{ dim, n_eq, n_in }; // creating QP object + qp2.settings.initial_guess = proxqp::InitialGuessStatus::NO_INITIAL_GUESS; + DOCTEST_CHECK(qp2.settings.initial_guess == + proxqp::InitialGuessStatus::NO_INITIAL_GUESS); + qp2.settings.eps_abs = eps_abs; + qp2.settings.eps_rel = 0; + qp2.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + nullopt, + mu_eq); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + qp2.solve(); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + + for (isize iter = 0; iter < 10; ++iter) { + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + qp2.solve(); + DOCTEST_CHECK(std::abs(mu_eq - qp2.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp2.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp2.results.info.mu_eq_inv) <= 1.E-9); + pri_res = std::max( + (qp_random.A * qp2.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp2.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp2.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp2.results.x + qp_random.g + + qp_random.A.transpose() * qp2.results.y + + qp_random.C.transpose() * qp2.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + // conter factual check with another QP object starting at the updated model + pod::QP qp3{ dim, n_eq, n_in }; // creating QP object + qp3.settings.initial_guess = proxqp::InitialGuessStatus::NO_INITIAL_GUESS; + DOCTEST_CHECK(qp3.settings.initial_guess == + proxqp::InitialGuessStatus::NO_INITIAL_GUESS); + qp3.settings.eps_abs = eps_abs; + qp3.settings.eps_rel = 0; + qp3.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + compute_preconditioner, + rho, + mu_eq); + + for (isize iter = 0; iter < 10; ++iter) { + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + DOCTEST_CHECK(std::abs(rho - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(rho - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(mu_eq - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(T(1) / mu_eq - qp3.results.info.mu_eq_inv) <= 1.E-9); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } + + qp3.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + compute_preconditioner, + 1.e-6, + 1.e-3); + for (isize iter = 0; iter < 10; ++iter) { + DOCTEST_CHECK(std::abs(1.e-6 - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e3 - qp3.results.info.mu_eq_inv) <= 1.E-9); + qp3.solve(); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.settings.default_rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-6 - qp3.results.info.rho) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.settings.default_mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e-3 - qp3.results.info.mu_eq) <= 1.E-9); + DOCTEST_CHECK(std::abs(1.e3 - qp3.results.info.mu_eq_inv) <= 1.E-9); + pri_res = std::max( + (qp_random.A * qp3.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp3.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp3.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp3.results.x + qp_random.g + + qp_random.A.transpose() * qp3.results.y + + qp_random.C.transpose() * qp3.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + } +} + +TEST_CASE("ProxQP::dense: init must be called before update") +{ + + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + utils::rand::set_seed(1); + dense::isize dim = 10; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + + // call update without init, update calls init internally + qp.update(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true); + + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + + qp_random.H *= 2.; + qp_random.g = utils::rand::vector_rand(dim); + qp.update(qp_random.H, + qp_random.g, + nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + true); + + qp.solve(); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); +} +// test of the box constraints interface +TEST_CASE("ProxQP::dense: check ordering of z when there are box constraints") +{ + dense::isize n_test(1000); + double sparsity_factor = 1.; + T eps_abs = T(1e-9); + dense::isize dim = 15; + + // mixing ineq and box constraints + for (isize i = 0; i < n_test; i++) { + utils::rand::set_seed(i); + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + // ineq and boxes + Eigen::Matrix x_sol = + utils::rand::vector_rand(dim); + Eigen::Matrix delta(n_in); + for (proxqp::isize i = 0; i < n_in; ++i) { + delta(i) = utils::rand::uniform_rand(); + } + qp_random.u = qp_random.C * x_sol + delta; + qp_random.b = qp_random.A * x_sol; + Eigen::Matrix u_box(dim); + u_box.setZero(); + Eigen::Matrix l_box(dim); + l_box.setZero(); + for (proxqp::isize i = 0; i < dim; ++i) { + T shift = utils::rand::uniform_rand(); + u_box(i) = x_sol(i) + shift; + l_box(i) = x_sol(i) - shift; + } + /////////////////// for debuging + // using Mat = + // Eigen::Matrix; + // Mat C_enlarged(dim+n_in,dim); + // C_enlarged.setZero(); + // C_enlarged.topLeftCorner(n_in,dim) = qp_random.C; + // C_enlarged.bottomLeftCorner(dim,dim).diagonal().array() += 1.; + // Eigen::Matrix u_enlarged(n_in+dim); + // Eigen::Matrix l_enlarged(n_in+dim); + // u_enlarged.head(n_in) = qp_random.u; + // u_enlarged.tail(dim) = u_box; + // l_enlarged.head(n_in) = qp_random.l; + // l_enlarged.tail(dim) = l_box; + // std::cout << "n " << dim << " n_eq " << n_eq << " n_in "<< n_in << + // std::endl; std::cout << "=================qp compare" << std::endl; + // pod::QP qp_compare{ dim, n_eq, dim + n_in, false}; + // qp_compare.settings.eps_abs = eps_abs; + // qp_compare.settings.eps_rel = 0; + // qp_compare.settings.max_iter = 10; + // qp_compare.settings.max_iter_in = 10; + // qp_compare.settings.verbose = true; + // qp_compare.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + // qp_compare.init(qp_random.H, + // qp_random.g, + // qp_random.A, + // qp_random.b, + // C_enlarged, + // l_enlarged, + // u_enlarged, + // true); + // qp_compare.solve(); + // std::cout << "=================qp compare end" << std::endl; + //////////////// + + pod::QP qp(dim, n_eq, n_in, true); + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + l_box, + u_box, + true); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + pri_res = std::max(pri_res, + (helpers::positive_part(qp.results.x - u_box) + + helpers::negative_part(qp.results.x - l_box)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z.head(n_in) + + qp.results.z.tail(dim)) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + } + // idem but without ineq constraints + for (isize i = 0; i < n_test; i++) { + utils::rand::set_seed(i); + dense::isize n_eq(dim / 4); + dense::isize n_in(0); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + // ineq and boxes + Eigen::Matrix x_sol = + utils::rand::vector_rand(dim); + Eigen::Matrix delta(n_in); + for (proxqp::isize i = 0; i < n_in; ++i) { + delta(i) = utils::rand::uniform_rand(); + } + qp_random.u = qp_random.C * x_sol + delta; + qp_random.b = qp_random.A * x_sol; + Eigen::Matrix u_box(dim); + u_box.setZero(); + Eigen::Matrix l_box(dim); + l_box.setZero(); + for (proxqp::isize i = 0; i < dim; ++i) { + T shift = utils::rand::uniform_rand(); + u_box(i) = x_sol(i) + shift; + l_box(i) = x_sol(i) - shift; + } + + pod::QP qp(dim, n_eq, n_in, true); + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + l_box, + u_box); + + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + pri_res = std::max(pri_res, + (helpers::positive_part(qp.results.x - u_box) + + helpers::negative_part(qp.results.x - l_box)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z.head(n_in) + + qp.results.z.tail(dim)) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + } + // idem but without ineq and without eq constraints + for (isize i = 0; i < n_test; i++) { + dense::isize n_eq(0); + dense::isize n_in(0); + T strong_convexity_factor(1.e-2); + using Mat = + Eigen::Matrix; + Mat eye(dim, dim); + eye.setZero(); + eye.diagonal().array() += 1.; + + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + // ineq and boxes + Eigen::Matrix x_sol = + utils::rand::vector_rand(dim); + Eigen::Matrix delta(n_in); + for (proxqp::isize i = 0; i < n_in; ++i) { + delta(i) = utils::rand::uniform_rand(); + } + qp_random.u = qp_random.C * x_sol + delta; + qp_random.b = qp_random.A * x_sol; + Eigen::Matrix u_box(dim); + u_box.setZero(); + Eigen::Matrix l_box(dim); + l_box.setZero(); + for (proxqp::isize i = 0; i < dim; ++i) { + T shift = utils::rand::uniform_rand(); + u_box(i) = x_sol(i) + shift; + l_box(i) = x_sol(i) - shift; + } + // make a qp to compare + pod::QP qp_compare(dim, n_eq, dim, false); + qp_compare.settings.eps_abs = eps_abs; + qp_compare.settings.eps_rel = 0; + qp_compare.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + qp_compare.settings.compute_preconditioner = true; + qp_compare.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + eye, + l_box, + u_box, + true); + + qp_compare.solve(); + + T pri_res = std::max( + (qp_random.A * qp_compare.results.x - qp_random.b) + .lpNorm(), + (helpers::positive_part(qp_random.C * qp_compare.results.x - + qp_random.u) + + helpers::negative_part(qp_random.C * qp_compare.results.x - qp_random.l)) + .lpNorm()); + pri_res = std::max(pri_res, + (helpers::positive_part(qp_compare.results.x - u_box) + + helpers::negative_part(qp_compare.results.x - l_box)) + .lpNorm()); + T dua_res = (qp_random.H * qp_compare.results.x + qp_random.g + + qp_random.C.transpose() * qp_compare.results.z.head(n_in) + + qp_random.A.transpose() * qp_compare.results.y + + qp_compare.results.z.tail(dim)) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + // ineq and boxes + pod::QP qp(dim, n_eq, n_in, true); + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + qp.settings.compute_preconditioner = true; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + l_box, + u_box, + true); + + qp.solve(); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + pri_res = std::max(pri_res, + (helpers::positive_part(qp.results.x - u_box) + + helpers::negative_part(qp.results.x - l_box)) + .lpNorm()); + dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z.head(n_in) + + qp.results.z.tail(dim)) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + } +} +TEST_CASE("ProxQP::dense: check updates work when there are box constraints") +{ + + double sparsity_factor = 1.; + T eps_abs = T(1e-9); + dense::isize dim = 50; + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + // ineq and boxes + pod::QP qp(dim, n_eq, n_in, true); + Eigen::Matrix u_box(dim); + u_box.setZero(); + u_box.array() += 1.E2; + Eigen::Matrix l_box(dim); + l_box.setZero(); + l_box.array() -= 1.E2; + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + l_box, + u_box); + + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + pri_res = std::max(pri_res, + (helpers::positive_part(qp.results.x - u_box) + + helpers::negative_part(qp.results.x - l_box)) + .lpNorm()); + T dua_res = + (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z.head(n_in) + qp.results.z.tail(dim)) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); + + u_box.array() += 1.E1; + l_box.array() -= 1.E1; + + qp.update(nullopt, + nullopt, + nullopt, + nullopt, + nullopt, + qp_random.l, + qp_random.u, + l_box, + u_box); + + qp.solve(); + + pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + pri_res = std::max(pri_res, + (helpers::positive_part(qp.results.x - u_box) + + helpers::negative_part(qp.results.x - l_box)) + .lpNorm()); + dua_res = + (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z.head(n_in) + qp.results.z.tail(dim)) + .lpNorm(); + CHECK(dua_res <= eps_abs); + CHECK(pri_res <= eps_abs); +} +TEST_CASE("ProxQP::dense: test primal infeasibility solving") +{ + double sparsity_factor = 0.15; + T eps_abs = T(1e-5); + utils::rand::set_seed(1); + dense::isize dim = 20; + + dense::isize n_eq(dim / 4); + dense::isize n_in(dim / 4); + T strong_convexity_factor(1.e-2); + for (isize i = 0; i < 20; ++i) { + ::proxsuite::proxqp::utils::rand::set_seed(i); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + pod::QP qp(dim, n_eq, n_in); + qp.settings.eps_abs = eps_abs; + qp.settings.eps_rel = 0; + // create infeasible problem + qp_random.b.array() += T(10.); + qp_random.u.array() -= T(100.); + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + qp.settings.primal_infeasibility_solving = true; + qp.settings.eps_primal_inf = T(1.E-4); + qp.settings.eps_dual_inf = T(1.E-4); + qp.settings.verbose = true; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + proxsuite::proxqp::utils::Vec rhs_dim(dim); + proxsuite::proxqp::utils::Vec rhs_n_eq(n_eq); + rhs_n_eq.setOnes(); + proxsuite::proxqp::utils::Vec rhs_n_in(n_in); + rhs_n_in.setOnes(); + rhs_dim.noalias() = + qp_random.A.transpose() * rhs_n_eq + qp_random.C.transpose() * rhs_n_in; + T scaled_eps = (rhs_dim).lpNorm() * eps_abs; + + T pri_res = + (qp_random.A.transpose() * (qp_random.A * qp.results.x - qp_random.b) + + qp_random.C.transpose() * + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l))) + .lpNorm(); + T dua_res = (qp_random.H.selfadjointView() * qp.results.x + + qp_random.g + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= scaled_eps); + DOCTEST_CHECK(dua_res <= eps_abs); + } +} + +TEST_CASE("ProxQP::dense: estimate of minimal eigenvalues using Eigen") +{ + double sparsity_factor = 1.; + T tol = T(1e-6); + utils::rand::set_seed(1); + dense::isize dim = 2; + dense::isize n_eq(dim); + dense::isize n_in(dim); + T strong_convexity_factor(1.e-2); + for (isize i = 0; i < 1; ++i) { + // trivial test + ::proxsuite::proxqp::utils::rand::set_seed(i); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + qp_random.H.setZero(); + qp_random.H.diagonal().setOnes(); + qp_random.H.diagonal().tail(1).setConstant(-1.); + + T estimate_minimal_eigen_value = + dense::estimate_minimal_eigen_value_of_symmetric_matrix( + qp_random.H, EigenValueEstimateMethodOption::ExactMethod, 1.E-6, 10000); + + pod::QP qp(dim, n_eq, n_in); + qp.settings.max_iter = 1; + qp.settings.max_iter_in = 1; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + nullopt, + nullopt, + nullopt, + estimate_minimal_eigen_value); + + DOCTEST_CHECK(std::abs(qp.results.info.minimal_H_eigenvalue_estimate + 1) <= + tol); + } + dim = 50; + n_eq = dim; + n_in = dim; + for (isize i = 0; i < 20; ++i) { + ::proxsuite::proxqp::utils::rand::set_seed(i); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + qp_random.H.setZero(); + dense::Vec random_diag = proxqp::utils::rand::vector_rand(dim); + qp_random.H.diagonal().array() += random_diag.array(); + T minimal_eigenvalue = qp_random.H.diagonal().minCoeff(); + + T estimate_minimal_eigen_value = + dense::estimate_minimal_eigen_value_of_symmetric_matrix( + qp_random.H, EigenValueEstimateMethodOption::ExactMethod, 1.E-6, 10000); + + pod::QP qp(dim, n_eq, n_in); + qp.settings.max_iter = 1; + qp.settings.max_iter_in = 1; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + nullopt, + nullopt, + nullopt, + estimate_minimal_eigen_value); + DOCTEST_CHECK(std::abs(qp.results.info.minimal_H_eigenvalue_estimate - + minimal_eigenvalue) <= tol); + } + dim = 50; + n_eq = dim; + n_in = dim; + for (isize i = 0; i < 20; ++i) { + ::proxsuite::proxqp::utils::rand::set_seed(i); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + dense::Vec random_diag = proxqp::utils::rand::vector_rand(dim); + qp_random.H.diagonal().array() += 100 * random_diag.array(); + Eigen::SelfAdjointEigenSolver> es(qp_random.H, + Eigen::EigenvaluesOnly); + T minimal_eigenvalue = T(es.eigenvalues().minCoeff()); + + T estimate_minimal_eigen_value = + dense::estimate_minimal_eigen_value_of_symmetric_matrix( + qp_random.H, EigenValueEstimateMethodOption::ExactMethod, 1.E-6, 10000); + + pod::QP qp(dim, n_eq, n_in); + qp.settings.max_iter = 1; + qp.settings.max_iter_in = 1; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + nullopt, + nullopt, + nullopt, + estimate_minimal_eigen_value); + + DOCTEST_CHECK(std::abs(qp.results.info.minimal_H_eigenvalue_estimate - + minimal_eigenvalue) <= tol); + } +} + +TEST_CASE( + "ProxQP::dense: test estimate of minimal eigenvalue using manual choice") +{ + double sparsity_factor = 1.; + T tol = T(1e-6); + utils::rand::set_seed(1); + dense::isize dim = 2; + dense::isize n_eq(dim); + dense::isize n_in(dim); + T strong_convexity_factor(1.e-2); + for (isize i = 0; i < 1; ++i) { + // trivial test + ::proxsuite::proxqp::utils::rand::set_seed(i); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + qp_random.H.setZero(); + qp_random.H.diagonal().setOnes(); + qp_random.H.diagonal().tail(1).setConstant(-1.); + + pod::QP qp(dim, n_eq, n_in); + qp.settings.max_iter = 1; + qp.settings.max_iter_in = 1; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + nullopt, + nullopt, + nullopt, + -1); + + DOCTEST_CHECK(std::abs(qp.results.info.minimal_H_eigenvalue_estimate + 1) <= + tol); + } + dim = 50; + n_eq = dim; + n_in = dim; + for (isize i = 0; i < 20; ++i) { + ::proxsuite::proxqp::utils::rand::set_seed(i); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + qp_random.H.setZero(); + dense::Vec random_diag = proxqp::utils::rand::vector_rand(dim); + qp_random.H.diagonal().array() += random_diag.array(); + T minimal_eigenvalue = qp_random.H.diagonal().minCoeff(); + + pod::QP qp(dim, n_eq, n_in); + qp.settings.max_iter = 1; + qp.settings.max_iter_in = 1; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + nullopt, + nullopt, + nullopt, + minimal_eigenvalue); + DOCTEST_CHECK(std::abs(qp.results.info.minimal_H_eigenvalue_estimate - + minimal_eigenvalue) <= tol); + } + dim = 50; + n_eq = dim; + n_in = dim; + for (isize i = 0; i < 20; ++i) { + ::proxsuite::proxqp::utils::rand::set_seed(i); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + dense::Vec random_diag = proxqp::utils::rand::vector_rand(dim); + qp_random.H.diagonal().array() += 100 * random_diag.array(); + Eigen::SelfAdjointEigenSolver> es(qp_random.H, + Eigen::EigenvaluesOnly); + T minimal_eigenvalue = T(es.eigenvalues().minCoeff()); + + pod::QP qp(dim, n_eq, n_in); + qp.settings.max_iter = 1; + qp.settings.max_iter_in = 1; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + nullopt, + nullopt, + nullopt, + minimal_eigenvalue); + + DOCTEST_CHECK(std::abs(qp.results.info.minimal_H_eigenvalue_estimate - + minimal_eigenvalue) <= tol); + } +} + +TEST_CASE( + "ProxQP::dense: test estimate of minimal eigenvalue using power iteration") +{ + double sparsity_factor = 1.; + T tol = T(1e-3); + utils::rand::set_seed(1); + dense::isize dim = 2; + dense::isize n_eq(dim); + dense::isize n_in(dim); + T strong_convexity_factor(1.e-2); + for (isize i = 0; i < 1; ++i) { + // trivial test + ::proxsuite::proxqp::utils::rand::set_seed(i); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + qp_random.H.setZero(); + qp_random.H.diagonal().setOnes(); + qp_random.H.diagonal().tail(1).setConstant(-0.5); + + T estimate_minimal_eigen_value = + dense::estimate_minimal_eigen_value_of_symmetric_matrix( + qp_random.H, + EigenValueEstimateMethodOption::PowerIteration, + 1.E-6, + 10000); + + pod::QP qp(dim, n_eq, n_in); + qp.settings.max_iter = 1; + qp.settings.max_iter_in = 1; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + nullopt, + nullopt, + nullopt, + estimate_minimal_eigen_value); + + DOCTEST_CHECK( + std::abs(qp.results.info.minimal_H_eigenvalue_estimate + 0.5) <= tol); + } + dim = 50; + n_eq = dim; + n_in = dim; + for (isize i = 0; i < 20; ++i) { + ::proxsuite::proxqp::utils::rand::set_seed(i); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + qp_random.H.setZero(); + dense::Vec random_diag = proxqp::utils::rand::vector_rand(dim); + qp_random.H.diagonal().array() += random_diag.array(); + T minimal_eigenvalue = qp_random.H.diagonal().minCoeff(); + + T estimate_minimal_eigen_value = + dense::estimate_minimal_eigen_value_of_symmetric_matrix( + qp_random.H, + EigenValueEstimateMethodOption::PowerIteration, + 1.E-6, + 10000); + + pod::QP qp(dim, n_eq, n_in); + qp.settings.max_iter = 1; + qp.settings.max_iter_in = 1; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + nullopt, + nullopt, + nullopt, + estimate_minimal_eigen_value); + DOCTEST_CHECK(std::abs(qp.results.info.minimal_H_eigenvalue_estimate - + minimal_eigenvalue) <= tol); + } + dim = 50; + n_eq = dim; + n_in = dim; + for (isize i = 0; i < 20; ++i) { + ::proxsuite::proxqp::utils::rand::set_seed(i); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + dense::Vec random_diag = proxqp::utils::rand::vector_rand(dim); + qp_random.H.diagonal().array() += + 100 * random_diag.array(); // add some random values to dense matrix + Eigen::SelfAdjointEigenSolver> es(qp_random.H, + Eigen::EigenvaluesOnly); + T minimal_eigenvalue = T(es.eigenvalues().minCoeff()); + + T estimate_minimal_eigen_value = + dense::estimate_minimal_eigen_value_of_symmetric_matrix( + qp_random.H, + EigenValueEstimateMethodOption::PowerIteration, + 1.E-6, + 10000); + + pod::QP qp(dim, n_eq, n_in); + qp.settings.max_iter = 1; + qp.settings.max_iter_in = 1; + qp.settings.initial_guess = InitialGuessStatus::NO_INITIAL_GUESS; + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u, + true, + nullopt, + nullopt, + nullopt, + estimate_minimal_eigen_value); + + DOCTEST_CHECK(std::abs(qp.results.info.minimal_H_eigenvalue_estimate - + minimal_eigenvalue) <= tol); + } +} + +DOCTEST_TEST_CASE("check that model.is_valid function for symmetric matrices " + "works for epsilon precision") +{ + Eigen::Matrix matrix = Eigen::Matrix::Random(); + Eigen::Matrix symmetric_mat = matrix + matrix.transpose(); + + symmetric_mat(0, 1) = + symmetric_mat(1, 0) + std::numeric_limits::epsilon(); + + // compare the two checks for symmetry with and without tolerance + bool is_symmetric_without_tolerance = + symmetric_mat.isApprox(symmetric_mat.transpose(), 0.0); + bool is_symmetric_with_tolerance = symmetric_mat.isApprox( + symmetric_mat.transpose(), + std::numeric_limits::epsilon()); + DOCTEST_CHECK(is_symmetric_without_tolerance == false); + DOCTEST_CHECK(is_symmetric_with_tolerance == true); + + // initialize a model with a symmetric matrix as Hessian, this runs + // model.is_valid() that performs the check above + pod::QP qp(3, 0, 0); + qp.init(symmetric_mat, nullopt, nullopt, nullopt, nullopt, nullopt, nullopt); +} + +TEST_CASE("ProxQP::dense: test memory allocation when estimating biggest " + "eigenvalue with power iteration") +{ + double sparsity_factor = 1.; + utils::rand::set_seed(1); + dense::isize dim = 2; + dense::isize n_eq(dim); + dense::isize n_in(dim); + T strong_convexity_factor(1.e-2); + Eigen::Matrix H; + Eigen::VectorXd dw(2), rhs(2), err_v(2); + // trivial test + ::proxsuite::proxqp::utils::rand::set_seed(1234); + proxqp::dense::Model qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + qp_random.H.setZero(); + qp_random.H.diagonal().setOnes(); + qp_random.H.diagonal().tail(1).setConstant(-0.5); + H = qp_random.H; + PROXSUITE_EIGEN_MALLOC_NOT_ALLOWED(); + dense::power_iteration(H, dw, rhs, err_v, 1.E-6, 10000); + PROXSUITE_EIGEN_MALLOC_ALLOWED(); +} + +// TEST_CASE("ProxQP::dense: sparse random strongly convex qp with" +// "inequality constraints: test PrimalLDLT backend mu update") +// { + +// std::cout << "---testing sparse random strongly convex qp with" +// "inequality constraints: test PrimalLDLT backend mu update---" +// << std::endl; +// double sparsity_factor = 1; +// utils::rand::set_seed(1); +// isize dim = 3; +// isize n_eq(0); +// isize n_in(9); +// T strong_convexity_factor(1.e-2); +// proxqp::dense::Model qp_random = +// proxqp::utils::dense_strongly_convex_qp( +// dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); +// pod::QP qp{ +// dim, +// n_eq, +// n_in, +// false, +// proxsuite::proxqp::HessianType::Dense, +// proxsuite::proxqp::DenseBackend::PrimalLDLT +// }; // creating QP object +// T eps_abs = T(1e-7); +// qp.settings.eps_abs = eps_abs; +// qp.settings.eps_rel = 0; +// qp.settings.compute_timings = true; +// qp.settings.verbose = true; +// qp.init(qp_random.H, +// qp_random.g, +// nullopt, +// nullopt, +// qp_random.C, +// nullopt, +// qp_random.u); +// qp.solve(); + +// DOCTEST_CHECK(qp.results.info.mu_updates > 0); + +// T pri_res = (helpers::negative_part(qp_random.C * qp.results.x - +// qp_random.l)) +// .lpNorm(); +// T dua_res = (qp_random.H * qp.results.x + qp_random.g + +// qp_random.C.transpose() * qp.results.z) +// .lpNorm(); +// DOCTEST_CHECK(pri_res <= eps_abs); +// DOCTEST_CHECK(dua_res <= eps_abs); + +// std::cout << "------using API solving qp with dim: " << dim +// << " neq: " << n_eq << " nin: " << n_in << std::endl; +// std::cout << "primal residual: " << pri_res << std::endl; +// std::cout << "dual residual: " << dua_res << std::endl; +// std::cout << "total number of iteration: " << qp.results.info.iter +// << std::endl; +// std::cout << "setup timing " << qp.results.info.setup_time << " solve time +// " +// << qp.results.info.solve_time << std::endl; +// } \ No newline at end of file diff --git a/test/src/osqp_dense_ruiz_equilibration.cpp b/test/src/osqp_dense_ruiz_equilibration.cpp new file mode 100644 index 000000000..01634b459 --- /dev/null +++ b/test/src/osqp_dense_ruiz_equilibration.cpp @@ -0,0 +1,75 @@ +// +// Copyright (c) 2022-2023 INRIA +// +#include +#include +#include +#include +#include +#include +#include + +using namespace proxsuite; +using Scalar = double; +namespace pod = proxsuite::osqp::dense; + +DOCTEST_TEST_CASE("ruiz preconditioner") +{ + int dim = 5; + int n_eq = 6; + int n_in = 0; + auto sym = proxqp::Symmetry::general; // 0 : upper triangular (by default), + // 1: + // auto sym = proxqp::Symmetry::lower; // 0 : upper triangular (by default), + // 1: lower triangular ; else full matrix + + Scalar sparsity_factor(0.75); + Scalar strong_convexity_factor(0.01); + proxqp::dense::Model qp_random = + proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + + switch (sym) { + case proxqp::Symmetry::upper: { + qp_random.H = qp_random.H.triangularView(); + break; + } + case proxqp::Symmetry::lower: { + qp_random.H = qp_random.H.triangularView(); + break; + } + default: { + } + } + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.default_mu_eq = Scalar(1.E-2); + qp.settings.default_mu_in = Scalar(1.E1); + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + + auto head = Eigen::Matrix( + qp.ruiz.delta.head(dim).asDiagonal()); + auto tail = Eigen::Matrix( + qp.ruiz.delta.tail(n_eq).asDiagonal()); + auto c = qp.ruiz.c; + + auto const& H = qp_random.H; + auto const& g = qp_random.g; + auto const& A = qp_random.A; + auto const& b = qp_random.b; + + auto H_new = (c * head * H * head).eval(); + auto g_new = (c * head * g).eval(); + auto A_new = (tail * A * head).eval(); + auto b_new = (tail * b).eval(); + + DOCTEST_CHECK((H_new - qp.work.H_scaled).norm() <= Scalar(1e-10)); + DOCTEST_CHECK((g_new - qp.work.g_scaled).norm() <= Scalar(1e-10)); + DOCTEST_CHECK((A_new - qp.work.A_scaled).norm() <= Scalar(1e-10)); + DOCTEST_CHECK((b_new - qp.work.b_scaled).norm() <= Scalar(1e-10)); +} diff --git a/test/src/osqp_dense_unconstrained_qp.cpp b/test/src/osqp_dense_unconstrained_qp.cpp new file mode 100644 index 000000000..8a7b5be74 --- /dev/null +++ b/test/src/osqp_dense_unconstrained_qp.cpp @@ -0,0 +1,218 @@ +// +// Copyright (c) 2022 INRIA +// +#include +#include +#include +#include +#include +#include +#include + +using namespace proxsuite; +using T = double; +namespace pod = proxsuite::osqp::dense; + +DOCTEST_TEST_CASE( + "sparse random strongly convex unconstrained qp and increasing dimension") +{ + + std::cout << "---testing sparse random strongly convex qp with increasing " + "dimension---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + for (int dim = 10; dim < 1000; dim += 100) { + + int n_eq(0); + int n_in(0); + T strong_convexity_factor(1.e-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_unconstrained_qp( + dim, sparsity_factor, strong_convexity_factor); + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.default_mu_eq = T(1.E-2); + qp.settings.default_mu_in = T(1.E1); + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------solving qp with dim: " << dim << " neq: " << n_eq + << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter_ext + << std::endl; + } +} + +DOCTEST_TEST_CASE("sparse random not strongly convex unconstrained qp and " + "increasing dimension") +{ + + std::cout << "---testing sparse random not strongly convex unconstrained qp " + "with increasing dimension---" + << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + for (int dim = 10; dim < 1000; dim += 100) { + + int n_eq(0); + int n_in(0); + T strong_convexity_factor(0); + proxqp::dense::Model qp_random = proxqp::utils::dense_unconstrained_qp( + dim, sparsity_factor, strong_convexity_factor); + auto x_sol = proxqp::utils::rand::vector_rand(dim); + qp_random.g = + -qp_random.H * + x_sol; // to be dually feasible g must be in the image space of H + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.default_mu_eq = T(1.E-2); + qp.settings.default_mu_in = T(1.E1); + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------solving qp with dim: " << dim << " neq: " << n_eq + << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter_ext + << std::endl; + } +} + +DOCTEST_TEST_CASE("unconstrained qp with H = Id and g random") +{ + + std::cout << "---unconstrained qp with H = Id and g random---" << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + + int dim(100); + int n_eq(0); + int n_in(0); + T strong_convexity_factor(1.E-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_unconstrained_qp( + dim, sparsity_factor, strong_convexity_factor); + qp_random.H.setZero(); + qp_random.H.diagonal().array() += 1; + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.default_mu_eq = T(1.E-2); + qp.settings.default_mu_in = T(1.E1); + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------solving qp with dim: " << dim << " neq: " << n_eq + << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter_ext + << std::endl; +} + +DOCTEST_TEST_CASE("unconstrained qp with H = Id and g = 0") +{ + + std::cout << "---unconstrained qp with H = Id and g = 0---" << std::endl; + double sparsity_factor = 0.15; + T eps_abs = T(1e-9); + + int dim(100); + int n_eq(0); + int n_in(0); + T strong_convexity_factor(1.E-2); + proxqp::dense::Model qp_random = proxqp::utils::dense_unconstrained_qp( + dim, sparsity_factor, strong_convexity_factor); + qp_random.H.setZero(); + qp_random.H.diagonal().array() += 1; + qp_random.g.setZero(); + + pod::QP qp{ dim, n_eq, n_in }; // creating QP object + qp.settings.eps_abs = eps_abs; + qp.settings.default_mu_eq = T(1.E-2); + qp.settings.default_mu_in = T(1.E1); + qp.init(qp_random.H, + qp_random.g, + qp_random.A, + qp_random.b, + qp_random.C, + qp_random.l, + qp_random.u); + qp.solve(); + + T pri_res = std::max( + (qp_random.A * qp.results.x - qp_random.b).lpNorm(), + (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) + + helpers::negative_part(qp_random.C * qp.results.x - qp_random.l)) + .lpNorm()); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.A.transpose() * qp.results.y + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + DOCTEST_CHECK(pri_res <= eps_abs); + DOCTEST_CHECK(dua_res <= eps_abs); + + std::cout << "------solving qp with dim: " << dim << " neq: " << n_eq + << " nin: " << n_in << std::endl; + std::cout << "primal residual: " << pri_res << std::endl; + std::cout << "dual residual: " << dua_res << std::endl; + std::cout << "total number of iteration: " << qp.results.info.iter_ext + << std::endl; +}