Program Listing for File solver.hpp
↰ Return to documentation for file (include/proxsuite/proxqp/dense/solver.hpp)
//
// Copyright (c) 2022-2024 INRIA
//
#ifndef PROXSUITE_PROXQP_DENSE_SOLVER_HPP
#define PROXSUITE_PROXQP_DENSE_SOLVER_HPP
#include "proxsuite/fwd.hpp"
#include "proxsuite/proxqp/dense/views.hpp"
#include "proxsuite/proxqp/dense/linesearch.hpp"
#include "proxsuite/proxqp/dense/helpers.hpp"
#include "proxsuite/proxqp/dense/utils.hpp"
#include <cmath>
#include <Eigen/Sparse>
#include <iostream>
#include <fstream>
#include <proxsuite/linalg/veg/util/dynstack_alloc.hpp>
#include <proxsuite/linalg/dense/ldlt.hpp>
#include <chrono>
#include <iomanip>
namespace proxsuite {
namespace proxqp {
namespace dense {
template<typename T>
void
refactorize(const Model<T>& qpmodel,
Results<T>& qpresults,
Workspace<T>& qpwork,
const isize n_constraints,
const DenseBackend& dense_backend,
T rho_new)
{
if (!qpwork.constraints_changed && rho_new == qpresults.info.rho) {
return;
}
proxsuite::linalg::veg::dynstack::DynStackMut stack{
proxsuite::linalg::veg::from_slice_mut, qpwork.ldl_stack.as_mut()
};
switch (dense_backend) {
case DenseBackend::PrimalDualLDLT: {
qpwork.kkt.diagonal().head(qpmodel.dim).array() +=
rho_new - qpresults.info.rho;
qpwork.kkt.diagonal().segment(qpmodel.dim, qpmodel.n_eq).array() =
-qpresults.info.mu_eq;
qpwork.ldl.factorize(qpwork.kkt.transpose(), stack);
isize n = qpmodel.dim;
isize n_eq = qpmodel.n_eq;
isize n_in = qpmodel.n_in;
isize n_c = qpwork.n_c;
LDLT_TEMP_MAT(T, new_cols, n + n_eq + n_c, n_c, stack);
T mu_in_neg(-qpresults.info.mu_in);
for (isize i = 0; i < n_constraints; ++i) {
isize j = qpwork.current_bijection_map[i];
if (j < n_c) {
auto col = new_cols.col(j);
if (i >= n_in) {
// I_scaled = D which is the diagonal matrix
// scaling x
// col(i-n_in) = ruiz.delta[i-qpmodel.n_in];
col(i - n_in) = qpwork.i_scaled[i - qpmodel.n_in];
} else {
col.head(n) = qpwork.C_scaled.row(i);
}
col.segment(n, n_eq + n_c).setZero();
col(n + n_eq + j) = mu_in_neg;
}
}
qpwork.ldl.insert_block_at(n + n_eq, new_cols, stack);
} break;
case DenseBackend::PrimalLDLT: {
qpwork.kkt.noalias() =
qpwork.H_scaled + (qpwork.A_scaled.transpose() * qpwork.A_scaled) *
qpresults.info.mu_eq_inv;
qpwork.kkt.diagonal().array() += qpresults.info.rho;
for (isize i = 0; i < n_constraints; i++) {
if (qpwork.active_inequalities(i)) {
if (i >= qpmodel.n_in) {
// box constraints
qpwork.kkt(i - qpmodel.n_in, i - qpmodel.n_in) +=
std::pow(qpwork.i_scaled(i - qpmodel.n_in), 2) *
qpresults.info.mu_in_inv;
} else {
// generic ineq constraint
qpwork.kkt.noalias() += qpwork.C_scaled.row(i).transpose() *
qpwork.C_scaled.row(i) *
qpresults.info.mu_in_inv;
}
}
}
qpwork.ldl.factorize(qpwork.kkt.transpose(), stack);
} break;
case DenseBackend::Automatic:
break;
}
qpwork.constraints_changed = false;
}
template<typename T>
void
mu_update(const Model<T>& qpmodel,
Results<T>& qpresults,
Workspace<T>& qpwork,
isize n_constraints,
const DenseBackend& dense_backend,
T mu_eq_new,
T mu_in_new)
{
proxsuite::linalg::veg::dynstack::DynStackMut stack{
proxsuite::linalg::veg::from_slice_mut, qpwork.ldl_stack.as_mut()
};
isize n = qpmodel.dim;
isize n_eq = qpmodel.n_eq;
isize n_c = qpwork.n_c;
if ((n_eq + n_c) == 0) {
return;
}
switch (dense_backend) {
case DenseBackend::PrimalDualLDLT: {
LDLT_TEMP_VEC_UNINIT(T, rank_update_alpha, n_eq + n_c, stack);
rank_update_alpha.head(n_eq).setConstant(qpresults.info.mu_eq -
mu_eq_new);
rank_update_alpha.tail(n_c).setConstant(qpresults.info.mu_in - mu_in_new);
{
auto _indices = stack.make_new_for_overwrite(
proxsuite::linalg::veg::Tag<isize>{}, n_eq + n_c);
isize* indices = _indices.ptr_mut();
for (isize k = 0; k < n_eq; ++k) {
indices[k] = n + k;
}
for (isize k = 0; k < n_c; ++k) {
indices[n_eq + k] = n + n_eq + k;
}
qpwork.ldl.diagonal_update_clobber_indices(
indices, n_eq + n_c, rank_update_alpha, stack);
}
} break;
case DenseBackend::PrimalLDLT: {
// we refactorize there for the moment
proxsuite::linalg::veg::dynstack::DynStackMut stack{
proxsuite::linalg::veg::from_slice_mut,
qpwork.ldl_stack.as_mut(),
};
// qpwork.kkt.noalias() = qpwork.H_scaled + (qpwork.A_scaled.transpose() *
// qpwork.A_scaled) / mu_eq_new; qpwork.kkt.diagonal().array() +=
// qpresults.info.rho; for (isize i = 0; i < n_constraints; i++){
// if (qpwork.active_inequalities(i)){
// if (i >=qpmodel.n_in){
// // box constraints
// qpwork.kkt(i-qpmodel.n_in,i-qpmodel.n_in) +=
// std::pow(qpwork.i_scaled(i-qpmodel.n_in),2) / mu_in_new ;
// } else{
// // generic ineq constraint
// qpwork.kkt.noalias() += qpwork.C_scaled.row(i).transpose() *
// qpwork.C_scaled.row(i) / mu_in_new ;
// }
// }
// }
// qpwork.ldl.factorize(qpwork.kkt.transpose(), stack);
// mu update for C_J
{
LDLT_TEMP_MAT_UNINIT(T, new_cols, qpmodel.dim, qpwork.n_c, stack);
qpwork.dw_aug.head(qpmodel.dim).setOnes();
T delta_mu(T(1) / mu_in_new - qpresults.info.mu_in_inv);
qpwork.dw_aug.head(qpmodel.dim).array() *= delta_mu;
for (isize i = 0; i < n_constraints; ++i) {
isize j = qpwork.current_bijection_map[i];
if (j < n_c) {
auto col = new_cols.col(j);
if (i >= qpmodel.n_in) {
// box constraint
col.setZero();
col[i - qpmodel.n_in] = qpwork.i_scaled[i - qpmodel.n_in];
} else {
// generic ineq constraints
col = qpwork.C_scaled.row(i);
}
}
}
qpwork.ldl.rank_r_update(
new_cols, qpwork.dw_aug.head(qpwork.n_c), stack);
}
// mu update for A
{
LDLT_TEMP_MAT_UNINIT(T, new_cols, qpmodel.dim, qpmodel.n_eq, stack);
qpwork.dw_aug.head(qpmodel.n_eq).setOnes();
T delta_mu(1 / mu_eq_new - qpresults.info.mu_eq_inv);
qpwork.dw_aug.head(qpmodel.n_eq).array() *= delta_mu;
new_cols = qpwork.A_scaled.transpose();
qpwork.ldl.rank_r_update(
new_cols, qpwork.dw_aug.head(qpmodel.n_eq), stack);
}
} break;
case DenseBackend::Automatic:
break;
}
qpwork.constraints_changed = true;
}
template<typename T>
void
iterative_residual(const Model<T>& qpmodel,
Results<T>& qpresults,
Workspace<T>& qpwork,
const isize n_constraints,
isize inner_pb_dim,
const HessianType& hessian_type)
{
auto& Hdx = qpwork.Hdx;
auto& Adx = qpwork.Adx;
auto& ATdy = qpwork.CTz;
qpwork.err.head(inner_pb_dim) = qpwork.rhs.head(inner_pb_dim);
switch (hessian_type) {
case HessianType::Zero:
break;
case HessianType::Dense:
Hdx.noalias() = qpwork.H_scaled.template selfadjointView<Eigen::Lower>() *
qpwork.dw_aug.head(qpmodel.dim);
qpwork.err.head(qpmodel.dim).noalias() -= Hdx;
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() *
qpwork.dw_aug.head(qpmodel.dim).array();
qpwork.err.head(qpmodel.dim).noalias() -= Hdx;
break;
}
qpwork.err.head(qpmodel.dim) -=
qpresults.info.rho * qpwork.dw_aug.head(qpmodel.dim);
// PERF: fuse {A, C}_scaled multiplication operations
ATdy.noalias() = qpwork.A_scaled.transpose() *
qpwork.dw_aug.segment(qpmodel.dim, qpmodel.n_eq);
qpwork.err.head(qpmodel.dim).noalias() -= ATdy;
if (n_constraints > qpmodel.n_in) {
// there are box constraints
qpwork.active_part_z.tail(qpmodel.dim) = qpwork.dw_aug.head(qpmodel.dim);
qpwork.active_part_z.tail(qpmodel.dim).array() *= qpwork.i_scaled.array();
// ruiz.unscale_primal_in_place(VectorViewMut<T>{from_eigen,qpwork.active_part_z.tail(qpmodel.dim)});
}
for (isize i = 0; i < n_constraints; i++) {
isize j = qpwork.current_bijection_map(i);
if (j < qpwork.n_c) {
if (i >= qpmodel.n_in) {
// I_scaled * dz_box_scaled = unscale_primally(dz_box)
qpwork.err(i - qpmodel.n_in) -=
// qpwork.dw_aug(qpmodel.dim + qpmodel.n_eq + j) *
// ruiz.delta(i-qpmodel.n_in);
qpwork.dw_aug(qpmodel.dim + qpmodel.n_eq + j) *
qpwork.i_scaled(i - qpmodel.n_in);
// I_scaled * dx_scaled = dx_unscaled
qpwork.err(qpmodel.dim + qpmodel.n_eq + j) -=
(qpwork.active_part_z[i] -
qpwork.dw_aug(qpmodel.dim + qpmodel.n_eq + j) *
qpresults.info.mu_in);
} else {
qpwork.err.head(qpmodel.dim).noalias() -=
qpwork.dw_aug(qpmodel.dim + qpmodel.n_eq + j) *
qpwork.C_scaled.row(i);
qpwork.err(qpmodel.dim + qpmodel.n_eq + j) -=
(qpwork.C_scaled.row(i).dot(qpwork.dw_aug.head(qpmodel.dim)) -
qpwork.dw_aug(qpmodel.dim + qpmodel.n_eq + j) *
qpresults.info.mu_in);
}
}
}
Adx.noalias() = qpwork.A_scaled * qpwork.dw_aug.head(qpmodel.dim);
qpwork.err.segment(qpmodel.dim, qpmodel.n_eq).noalias() -= Adx;
qpwork.err.segment(qpmodel.dim, qpmodel.n_eq) +=
qpwork.dw_aug.segment(qpmodel.dim, qpmodel.n_eq) * qpresults.info.mu_eq;
}
template<typename T>
void
solve_linear_system(proxsuite::proxqp::dense::Vec<T>& dw,
const Model<T>& qpmodel,
Results<T>& qpresults,
Workspace<T>& qpwork,
const isize n_constraints,
const DenseBackend& dense_backend,
isize inner_pb_dim,
proxsuite::linalg::veg::dynstack::DynStackMut& stack)
{
switch (dense_backend) {
case DenseBackend::PrimalDualLDLT:
qpwork.ldl.solve_in_place(dw.head(inner_pb_dim), stack);
break;
case DenseBackend::PrimalLDLT:
// find dx
dw.head(qpmodel.dim).noalias() += qpresults.info.mu_eq_inv *
qpwork.A_scaled.transpose() *
dw.segment(qpmodel.dim, qpmodel.n_eq);
for (isize i = 0; i < n_constraints; i++) {
isize j = qpwork.current_bijection_map(i);
if (j < qpwork.n_c) {
if (i >= qpmodel.n_in) {
// box constraints
dw(i - qpmodel.n_in) += dw(j + qpmodel.dim + qpmodel.n_eq) *
qpwork.i_scaled(i - qpmodel.n_in);
} else {
// ineq constraints
dw.head(qpmodel.dim) +=
dw(j + qpmodel.dim + qpmodel.n_eq) * qpwork.C_scaled.row(i);
}
}
}
qpwork.ldl.solve_in_place(dw.head(qpmodel.dim), stack);
// find dy
dw.segment(qpmodel.dim, qpmodel.n_eq) -=
qpresults.info.mu_eq_inv * dw.segment(qpmodel.dim, qpmodel.n_eq);
dw.segment(qpmodel.dim, qpmodel.n_eq).noalias() +=
qpresults.info.mu_eq_inv *
(qpwork.A_scaled *
dw.head(
qpmodel.dim)); //- qpwork.rhs.segment(qpmodel.dim,qpmodel.n_eq));
// find dz_J
for (isize i = 0; i < n_constraints; i++) {
isize j = qpwork.current_bijection_map(i);
if (j < qpwork.n_c) {
if (i >= qpmodel.n_in) {
// box constraints
dw(j + qpmodel.dim + qpmodel.n_eq) -=
qpresults.info.mu_in_inv * (dw(j + qpmodel.dim + qpmodel.n_eq));
dw(j + qpmodel.dim + qpmodel.n_eq) +=
qpresults.info.mu_in_inv *
(dw(
i -
qpmodel.n_in)); //- qpwork.rhs(j + qpmodel.dim + qpmodel.n_eq));
} else {
// ineq constraints
dw(j + qpmodel.dim + qpmodel.n_eq) -=
qpresults.info.mu_in_inv * (dw(j + qpmodel.dim + qpmodel.n_eq));
dw(j + qpmodel.dim + qpmodel.n_eq) +=
qpresults.info.mu_in_inv *
(qpwork.C_scaled.row(i).dot(dw.head(
qpmodel.dim))); //- qpwork.rhs(j + qpmodel.dim + qpmodel.n_eq));
}
}
}
break;
case DenseBackend::Automatic:
break;
}
}
template<typename T>
void
iterative_solve_with_permut_fact( //
const Settings<T>& qpsettings,
const Model<T>& qpmodel,
Results<T>& qpresults,
Workspace<T>& qpwork,
const isize n_constraints,
const DenseBackend& dense_backend,
const HessianType& hessian_type,
T eps,
isize inner_pb_dim)
{
qpwork.err.setZero();
i32 it = 0;
i32 it_stability = 0;
proxsuite::linalg::veg::dynstack::DynStackMut stack{
proxsuite::linalg::veg::from_slice_mut, qpwork.ldl_stack.as_mut()
};
qpwork.dw_aug.head(inner_pb_dim) = qpwork.rhs.head(inner_pb_dim);
solve_linear_system(qpwork.dw_aug,
qpmodel,
qpresults,
qpwork,
n_constraints,
dense_backend,
inner_pb_dim,
stack);
iterative_residual<T>(
qpmodel, qpresults, qpwork, n_constraints, inner_pb_dim, hessian_type);
++it;
T preverr = infty_norm(qpwork.err.head(inner_pb_dim));
while (infty_norm(qpwork.err.head(inner_pb_dim)) >= eps) {
if (it >= qpsettings.nb_iterative_refinement) {
break;
}
++it;
solve_linear_system(qpwork.err,
qpmodel,
qpresults,
qpwork,
n_constraints,
dense_backend,
inner_pb_dim,
stack);
// qpwork.ldl.solve_in_place(qpwork.err.head(inner_pb_dim), stack);
qpwork.dw_aug.head(inner_pb_dim) += qpwork.err.head(inner_pb_dim);
qpwork.err.head(inner_pb_dim).setZero();
iterative_residual<T>(
qpmodel, qpresults, qpwork, n_constraints, inner_pb_dim, hessian_type);
if (infty_norm(qpwork.err.head(inner_pb_dim)) > preverr) {
it_stability += 1;
} else {
it_stability = 0;
}
if (it_stability == 2) {
break;
}
preverr = infty_norm(qpwork.err.head(inner_pb_dim));
}
if (infty_norm(qpwork.err.head(inner_pb_dim)) >=
std::max(eps, qpsettings.eps_refact)) {
refactorize(qpmodel,
qpresults,
qpwork,
n_constraints,
dense_backend,
qpresults.info.rho);
it = 0;
it_stability = 0;
qpwork.dw_aug.head(inner_pb_dim) = qpwork.rhs.head(inner_pb_dim);
solve_linear_system(qpwork.dw_aug,
qpmodel,
qpresults,
qpwork,
n_constraints,
dense_backend,
inner_pb_dim,
stack);
// qpwork.ldl.solve_in_place(qpwork.dw_aug.head(inner_pb_dim), stack);
iterative_residual<T>(
qpmodel, qpresults, qpwork, n_constraints, inner_pb_dim, hessian_type);
preverr = infty_norm(qpwork.err.head(inner_pb_dim));
++it;
while (infty_norm(qpwork.err.head(inner_pb_dim)) >= eps) {
if (it >= qpsettings.nb_iterative_refinement) {
break;
}
++it;
solve_linear_system(qpwork.err,
qpmodel,
qpresults,
qpwork,
n_constraints,
dense_backend,
inner_pb_dim,
stack);
// qpwork.ldl.solve_in_place(qpwork.err.head(inner_pb_dim), stack);
qpwork.dw_aug.head(inner_pb_dim) += qpwork.err.head(inner_pb_dim);
qpwork.err.head(inner_pb_dim).setZero();
iterative_residual<T>(
qpmodel, qpresults, qpwork, n_constraints, inner_pb_dim, hessian_type);
if (infty_norm(qpwork.err.head(inner_pb_dim)) > preverr) {
it_stability += 1;
} else {
it_stability = 0;
}
if (it_stability == 2) {
break;
}
preverr = infty_norm(qpwork.err.head(inner_pb_dim));
}
}
if (infty_norm(qpwork.err.head(inner_pb_dim)) >= eps && qpsettings.verbose) {
// std::cout << "after refact err " << err << std::endl;
std::cout << "refact err " << infty_norm(qpwork.err.head(inner_pb_dim))
<< std::endl;
}
qpresults.info.iterative_residual = infty_norm(qpwork.err.head(inner_pb_dim));
qpwork.rhs.head(inner_pb_dim).setZero();
}
template<typename T>
void
bcl_update(const Settings<T>& qpsettings,
Results<T>& qpresults,
Workspace<T>& qpwork,
T& primal_feasibility_lhs_new,
T& bcl_eta_ext,
T& bcl_eta_in,
T bcl_eta_ext_init,
T eps_in_min,
T& new_bcl_mu_in,
T& new_bcl_mu_eq,
T& new_bcl_mu_in_inv,
T& new_bcl_mu_eq_inv
)
{
if (primal_feasibility_lhs_new <= bcl_eta_ext ||
qpresults.info.iter > qpsettings.safe_guard) {
/* TO PUT IN DEBUG MODE
if (qpsettings.verbose) {
std::cout << "good step" << std::endl;
}
*/
bcl_eta_ext *= pow(qpresults.info.mu_in, qpsettings.beta_bcl);
bcl_eta_in = std::max(bcl_eta_in * qpresults.info.mu_in, eps_in_min);
} else {
/* TO PUT IN DEBUG MODE
if (qpsettings.verbose) {
std::cout << "bad step" << std::endl;
}
*/
qpresults.y = qpwork.y_prev;
qpresults.z = qpwork.z_prev;
new_bcl_mu_in = std::max(qpresults.info.mu_in * qpsettings.mu_update_factor,
qpsettings.mu_min_in);
new_bcl_mu_eq = std::max(qpresults.info.mu_eq * qpsettings.mu_update_factor,
qpsettings.mu_min_eq);
new_bcl_mu_in_inv =
std::min(qpresults.info.mu_in_inv * qpsettings.mu_update_inv_factor,
qpsettings.mu_max_in_inv);
new_bcl_mu_eq_inv =
std::min(qpresults.info.mu_eq_inv * qpsettings.mu_update_inv_factor,
qpsettings.mu_max_eq_inv);
bcl_eta_ext = bcl_eta_ext_init * pow(new_bcl_mu_in, qpsettings.alpha_bcl);
bcl_eta_in = std::max(new_bcl_mu_in, eps_in_min);
}
}
template<typename T>
void
Martinez_update(const Settings<T>& qpsettings,
Results<T>& qpresults,
T& primal_feasibility_lhs_new,
T& primal_feasibility_lhs_old,
T& bcl_eta_in,
T eps_in_min,
T& new_bcl_mu_in,
T& new_bcl_mu_eq,
T& new_bcl_mu_in_inv,
T& new_bcl_mu_eq_inv
)
{
bcl_eta_in = std::max(bcl_eta_in * 0.1, eps_in_min);
if (primal_feasibility_lhs_new <= 0.95 * primal_feasibility_lhs_old) {
/* TO PUT IN DEBUG MODE
if (qpsettings.verbose) {
std::cout << "good step" << std::endl;
}
*/
} else {
/* TO PUT IN DEBUG MODE
if (qpsettings.verbose) {
std::cout << "bad step" << std::endl;
}
*/
new_bcl_mu_in = std::max(qpresults.info.mu_in * qpsettings.mu_update_factor,
qpsettings.mu_min_in);
new_bcl_mu_eq = std::max(qpresults.info.mu_eq * qpsettings.mu_update_factor,
qpsettings.mu_min_eq);
new_bcl_mu_in_inv =
std::min(qpresults.info.mu_in_inv * qpsettings.mu_update_inv_factor,
qpsettings.mu_max_in_inv);
new_bcl_mu_eq_inv =
std::min(qpresults.info.mu_eq_inv * qpsettings.mu_update_inv_factor,
qpsettings.mu_max_eq_inv);
}
}
template<typename T>
auto
compute_inner_loop_saddle_point(const Model<T>& qpmodel,
Results<T>& qpresults,
Workspace<T>& qpwork,
const Settings<T>& qpsettings) -> T
{
qpwork.active_part_z =
helpers::positive_part(qpwork.primal_residual_in_scaled_up) +
helpers::negative_part(qpresults.si);
switch (qpsettings.merit_function_type) {
case MeritFunctionType::GPDAL:
qpwork.active_part_z -=
qpsettings.alpha_gpdal * qpresults.z *
qpresults.info.mu_in; // contains now : [Cx-u+z_prev*mu_in]+
// qpwork.active_part_z.head(qpmodel.n_in) -=
// qpsettings.alpha_gpdal * qpresults.z.head(qpmodel.n_in) *
// qpresults.info.mu_in; // contains now : [Cx-u+z_prev*mu_in]+
// if (box_constraints){
// qpwork.active_part_z.tail(qpmodel.dim) -=
// qpsettings.alpha_gpdal * qpresults.z.tail(qpmodel.dim) *
// qpresults.info.mu_in; // contains now : [Cx-u+z_prev*mu_in]+
// }
break;
case MeritFunctionType::PDAL:
qpwork.active_part_z -=
qpresults.z *
qpresults.info.mu_in; // contains now : [Cx-u+z_prev*mu_in]+
// + [Cx-l+z_prev*mu_in]- - z*mu_in
// qpwork.active_part_z.head(qpmodel.n_in) -=
// qpresults.z.head(qpmodel.n_in) *
// qpresults.info.mu_in; // contains now : [Cx-u+z_prev*mu_in]+
// // + [Cx-l+z_prev*mu_in]- - z*mu_in
// if (box_constraints){
// qpwork.active_part_z.tail(qpmodel.dim) -=
// qpresults.z.tail(qpmodel.dim) *
// qpresults.info.mu_in; // contains now : [Cx-u+z_prev*mu_in]+
// }
break;
}
T err = infty_norm(qpwork.active_part_z);
qpwork.err.segment(qpmodel.dim, qpmodel.n_eq) = qpresults.se;
// qpwork.primal_residual_eq_scaled; // contains now Ax-b-(y-y_prev)/mu
T prim_eq_e = infty_norm(
qpwork.err.segment(qpmodel.dim, qpmodel.n_eq)); // ||Ax-b-(y-y_prev)/mu||
err = std::max(err, prim_eq_e);
T dual_e =
infty_norm(qpwork.dual_residual_scaled); // contains ||Hx + rho(x-xprev) +
// g + Aty + Ctz||
err = std::max(err, dual_e);
return err;
}
template<typename T>
void
primal_dual_semi_smooth_newton_step(const Settings<T>& qpsettings,
const Model<T>& qpmodel,
Results<T>& qpresults,
Workspace<T>& qpwork,
const bool box_constraints,
const isize n_constraints,
const DenseBackend& dense_backend,
const HessianType& hessian_type,
T eps)
{
/* MUST BE
* dual_residual_scaled = Hx + rho * (x-x_prev) + A.T y + C.T z
* primal_residual_eq_scaled = Ax-b+mu_eq (y_prev-y)
* primal_residual_in_scaled_up = Cx-u+mu_in(z_prev)
* primal_residual_in_scaled_low = Cx-l+mu_in(z_prev)
*/
qpwork.active_set_up.array() =
(qpwork.primal_residual_in_scaled_up.array() >= 0);
// primal_residual_in_scaled_low = Cx-l+mu_in(z_prev) + mu_in(alpha_gdpal - 1)
// * z
qpwork.active_set_low.array() = (qpresults.si.array() <= 0);
qpwork.active_inequalities = qpwork.active_set_up || qpwork.active_set_low;
isize numactive_inequalities = qpwork.active_inequalities.count();
isize inner_pb_dim = qpmodel.dim + qpmodel.n_eq + numactive_inequalities;
qpwork.rhs.setZero();
qpwork.dw_aug.setZero();
linesearch::active_set_change(
qpmodel, qpresults, dense_backend, n_constraints, qpwork);
qpwork.rhs.head(qpmodel.dim) = -qpwork.dual_residual_scaled;
if (box_constraints) {
// use active_part_z as tmp variable in order to unscale primarilly z
// as I_scaled.T * z_scaled = unscale_primarilly_(z_scaled)
qpwork.active_part_z.tail(qpmodel.dim) = qpresults.z.tail(qpmodel.dim);
qpwork.active_part_z.tail(qpmodel.dim).array() *= qpwork.i_scaled.array();
// ruiz.unscale_primal_in_place(VectorViewMut<T>{from_eigen,qpwork.active_part_z.tail(qpmodel.dim)});
}
qpwork.rhs.segment(qpmodel.dim, qpmodel.n_eq) = -qpresults.se;
// -qpwork.primal_residual_eq_scaled;
switch (qpsettings.merit_function_type) {
case MeritFunctionType::GPDAL:
for (isize i = 0; i < n_constraints; i++) {
isize j = qpwork.current_bijection_map(i);
if (j < qpwork.n_c) {
if (qpwork.active_set_up(i)) {
qpwork.rhs(j + qpmodel.dim + qpmodel.n_eq) =
-qpwork.primal_residual_in_scaled_up(i) +
qpresults.z(i) * qpresults.info.mu_in * qpsettings.alpha_gpdal;
} else if (qpwork.active_set_low(i)) {
qpwork.rhs(j + qpmodel.dim + qpmodel.n_eq) =
-qpresults.si(i) +
qpresults.z(i) * qpresults.info.mu_in * qpsettings.alpha_gpdal;
}
} else {
// unactive unrelevant columns
if (i >= qpmodel.n_in) {
qpwork.rhs(i - qpmodel.n_in) += qpwork.active_part_z(i);
} else {
qpwork.rhs.head(qpmodel.dim) +=
qpresults.z(i) * qpwork.C_scaled.row(i);
}
}
}
break;
case MeritFunctionType::PDAL:
for (isize i = 0; i < n_constraints; i++) {
isize j = qpwork.current_bijection_map(i);
if (j < qpwork.n_c) {
if (qpwork.active_set_up(i)) {
qpwork.rhs(j + qpmodel.dim + qpmodel.n_eq) =
-qpwork.primal_residual_in_scaled_up(i) +
qpresults.z(i) * qpresults.info.mu_in;
} else if (qpwork.active_set_low(i)) {
qpwork.rhs(j + qpmodel.dim + qpmodel.n_eq) =
-qpresults.si(i) + qpresults.z(i) * qpresults.info.mu_in;
}
} else {
if (i >= qpmodel.n_in) {
// unactive unrelevant columns
qpwork.rhs(i - qpmodel.n_in) += qpwork.active_part_z(i);
} else {
qpwork.rhs.head(qpmodel.dim) +=
qpresults.z(i) * qpwork.C_scaled.row(i);
}
}
}
break;
}
iterative_solve_with_permut_fact( //
qpsettings,
qpmodel,
qpresults,
qpwork,
n_constraints,
dense_backend,
hessian_type,
eps,
inner_pb_dim);
// use active_part_z as a temporary variable to derive unpermutted dz step
for (isize j = 0; j < n_constraints; ++j) {
isize i = qpwork.current_bijection_map(j);
if (i < qpwork.n_c) {
qpwork.active_part_z(j) = qpwork.dw_aug(qpmodel.dim + qpmodel.n_eq + i);
} else {
qpwork.active_part_z(j) = -qpresults.z(j);
}
}
qpwork.dw_aug.tail(n_constraints) = qpwork.active_part_z;
}
template<typename T>
void
primal_dual_newton_semi_smooth(const Settings<T>& qpsettings,
const Model<T>& qpmodel,
Results<T>& qpresults,
Workspace<T>& qpwork,
const bool box_constraints,
const isize n_constraints,
preconditioner::RuizEquilibration<T>& ruiz,
const DenseBackend& dense_backend,
const HessianType& hessian_type,
T eps_int)
{
/* MUST CONTAIN IN ENTRY WITH x = x_prev ; y = y_prev ; z = z_prev
* dual_residual_scaled = Hx + rho * (x-x_prev) + A.T y + C.T z
* primal_residual_eq_scaled = Ax-b+mu_eq (y_prev-y)
* primal_residual_in_scaled_up = Cx-u+mu_in(z_prev)
* primal_residual_in_scaled_low = Cx-l+mu_in(z_prev)
*/
/* for debug
if (qpsettings.verbose) {
std::cout << "---- inner iteration inner error alpha ----" <<
std::endl;
}
*/
T err_in = 1.e6;
for (i64 iter = 0; iter <= qpsettings.max_iter_in; ++iter) {
if (iter == qpsettings.max_iter_in) {
qpresults.info.iter += qpsettings.max_iter_in + 1;
break;
}
proxsuite::linalg::veg::dynstack::DynStackMut stack{
proxsuite::linalg::veg::from_slice_mut, qpwork.ldl_stack.as_mut()
};
primal_dual_semi_smooth_newton_step<T>(qpsettings,
qpmodel,
qpresults,
qpwork,
box_constraints,
n_constraints,
dense_backend,
hessian_type,
eps_int);
auto& Hdx = qpwork.Hdx;
auto& Adx = qpwork.Adx;
auto& Cdx = qpwork.Cdx;
auto& ATdy = qpwork.CTz;
auto dx = qpwork.dw_aug.head(qpmodel.dim);
auto dy = qpwork.dw_aug.segment(qpmodel.dim, qpmodel.n_eq);
auto dz = qpwork.dw_aug.tail(n_constraints);
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) {
// use active_part_z as tmp variable in order to unscale primarilly dz
// as I_scaled.T * dz_scaled = unscale_primarilly_(dz_scaled)
// qpwork.active_part_z.tail(qpmodel.dim) = dz.tail(qpmodel.dim);
// ruiz.unscale_primal_in_place(VectorViewMut<T>{from_eigen,qpwork.active_part_z.tail(qpmodel.dim)});
// CTdz.noalias() += qpwork.active_part_z.tail(qpmodel.dim);
// Cdx.tail(qpmodel.dim) = dx;
// // I_scaled * dx_scaled = dx_unscaled
// ruiz.unscale_primal_in_place(VectorViewMut<T>{from_eigen,
// Cdx.tail(qpmodel.dim)});
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();
}
switch (qpsettings.merit_function_type) {
case MeritFunctionType::GPDAL:
Cdx.noalias() +=
(qpsettings.alpha_gpdal - 1.) * qpresults.info.mu_in * dz;
break;
case MeritFunctionType::PDAL:
break;
}
if (qpmodel.n_in > 0 || box_constraints) {
linesearch::primal_dual_ls(
qpmodel, qpresults, qpwork, qpsettings, n_constraints);
}
auto alpha = qpwork.alpha;
if (infty_norm(alpha * qpwork.dw_aug) < 1.E-11 && iter > 0) {
qpresults.info.iter += iter + 1;
break;
/* to put in debuger mode
if (qpsettings.verbose) {
std::cout << "infty_norm(alpha_step * dx) "
<< infty_norm(alpha *
qpwork.dw_aug) << std::endl;
}
*/
}
qpresults.x += alpha * dx;
// contains now : C(x+alpha dx)-u + z_prev * mu_in
qpwork.primal_residual_in_scaled_up += alpha * Cdx;
// contains now : C(x+alpha dx)-l + z_prev * mu_in
qpresults.si += alpha * Cdx;
// qpwork.primal_residual_eq_scaled +=
qpresults.se += alpha * (Adx - qpresults.info.mu_eq * dy);
qpresults.y += alpha * dy;
qpresults.z += alpha * dz;
switch (hessian_type) {
case HessianType::Zero:
qpwork.dual_residual_scaled +=
alpha * (qpresults.info.rho * dx + ATdy + CTdz);
break;
case HessianType::Dense:
qpwork.dual_residual_scaled +=
alpha * (qpresults.info.rho * dx + Hdx + ATdy + CTdz);
break;
case HessianType::Diagonal:
qpwork.dual_residual_scaled +=
alpha * (qpresults.info.rho * dx + Hdx + ATdy + CTdz);
break;
}
err_in = dense::compute_inner_loop_saddle_point(
qpmodel, qpresults, qpwork, qpsettings);
/* for debug
if (qpsettings.verbose) {
std::cout << " " << iter << " " <<
std::setprecision(2) << err_in
<< " " << alpha <<
std::endl;
}
*/
if (qpsettings.verbose) {
std::cout << "\033[1;34m[inner iteration " << iter + 1 << "]\033[0m"
<< std::endl;
std::cout << std::scientific << std::setw(2) << std::setprecision(2)
<< "| inner residual=" << err_in << " | alpha=" << alpha
<< std::endl;
}
if (iter % qpsettings.frequence_infeasibility_check == 0 ||
qpsettings.primal_infeasibility_solving) {
// compute primal and dual infeasibility criteria
bool is_primal_infeasible = global_primal_residual_infeasibility(
VectorViewMut<T>{ from_eigen, ATdy },
VectorViewMut<T>{ from_eigen, CTdz },
VectorViewMut<T>{ from_eigen, dy },
VectorViewMut<T>{ from_eigen, dz },
qpwork,
qpmodel,
qpsettings,
box_constraints,
ruiz);
bool is_dual_infeasible =
global_dual_residual_infeasibility(VectorViewMut<T>{ from_eigen, Adx },
VectorViewMut<T>{ from_eigen, Cdx },
VectorViewMut<T>{ from_eigen, Hdx },
VectorViewMut<T>{ from_eigen, dx },
qpwork,
qpsettings,
qpmodel,
box_constraints,
ruiz);
if (is_primal_infeasible) {
qpresults.info.status = QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE;
if (!qpsettings.primal_infeasibility_solving) {
qpresults.info.iter += iter + 1;
break;
}
} else if (is_dual_infeasible) {
qpresults.info.status = QPSolverOutput::PROXQP_DUAL_INFEASIBLE;
qpresults.info.iter += iter + 1;
break;
}
}
if (err_in <= eps_int) {
qpresults.info.iter += iter + 1;
break;
}
}
/* to put in debuger mode
if (qpsettings.verbose) {
if (err_in > eps_int){
std::cout << " inner loop residual is to high! Its value is equal to "
<< err_in << ", while it should be inferior to: " << eps_int << std::endl;
}
}
*/
}
template<typename T>
void
qp_solve( //
const Settings<T>& qpsettings,
const Model<T>& qpmodel,
Results<T>& qpresults,
Workspace<T>& qpwork,
const bool box_constraints,
const DenseBackend& dense_backend,
const HessianType& hessian_type,
preconditioner::RuizEquilibration<T>& ruiz)
{
/*** TEST WITH MATRIX FULL OF NAN FOR DEBUG
static constexpr Layout layout = rowmajor;
static constexpr auto DYN = Eigen::Dynamic;
using RowMat = Eigen::Matrix<T, DYN, DYN, Eigen::RowMajor>;
RowMat test(2,2); // test it is full of nan for debug
std::cout << "test " << test << std::endl;
*/
PROXSUITE_EIGEN_MALLOC_NOT_ALLOWED();
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: {
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: {
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: {
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: {
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);
T eps_in_min = std::min(qpsettings.eps_abs, T(1.E-9));
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);
T scaled_eps(qpsettings.eps_abs);
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_dual_feasible = dual_feasibility_lhs <= rhs_dua;
if (qpsettings.verbose) {
ruiz.unscale_primal_in_place(VectorViewMut<T>{ from_eigen, qpresults.x });
ruiz.unscale_dual_in_place_eq(
VectorViewMut<T>{ from_eigen, qpresults.y });
ruiz.unscale_dual_in_place_in(
VectorViewMut<T>{ from_eigen, qpresults.z.head(qpmodel.n_in) });
if (box_constraints) {
ruiz.unscale_box_dual_in_place_in(
VectorViewMut<T>{ 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<T>{ from_eigen, qpresults.x });
ruiz.scale_dual_in_place_eq(VectorViewMut<T>{ from_eigen, qpresults.y });
ruiz.scale_dual_in_place_in(
VectorViewMut<T>{ from_eigen, qpresults.z.head(qpmodel.n_in) });
if (box_constraints) {
ruiz.scale_box_dual_in_place_in(
VectorViewMut<T>{ 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<T>{ 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<T>{ 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)
}
primal_dual_newton_semi_smooth(qpsettings,
qpmodel,
qpresults,
qpwork,
box_constraints,
n_constraints,
ruiz,
dense_backend,
hessian_type,
bcl_eta_in);
if ((qpresults.info.status == QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE &&
!qpsettings.primal_infeasibility_solving) ||
qpresults.info.status == QPSolverOutput::PROXQP_DUAL_INFEASIBLE) {
// certificate of infeasibility
qpresults.x = qpwork.dw_aug.head(qpmodel.dim);
qpresults.y = qpwork.dw_aug.segment(qpmodel.dim, qpmodel.n_eq);
qpresults.z = qpwork.dw_aug.tail(n_constraints);
break;
}
if (scaled_eps == qpsettings.eps_abs &&
qpsettings.primal_infeasibility_solving &&
qpresults.info.status == QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) {
qpwork.rhs.segment(qpmodel.dim, qpmodel.n_eq + qpmodel.n_in)
.setConstant(T(1));
qpwork.rhs.head(qpmodel.dim).noalias() =
qpmodel.A.transpose() * qpwork.rhs.segment(qpmodel.dim, qpmodel.n_eq) +
qpmodel.C.transpose() *
qpwork.rhs.segment(qpmodel.dim + qpmodel.n_eq, qpmodel.n_in);
if (box_constraints) {
qpwork.rhs.head(qpmodel.dim).array() += qpwork.i_scaled.array();
}
scaled_eps =
infty_norm(qpwork.rhs.head(qpmodel.dim)) * qpsettings.eps_abs;
}
T primal_feasibility_lhs_new(primal_feasibility_lhs);
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,
qpwork,
primal_feasibility_lhs_new,
bcl_eta_ext,
bcl_eta_in,
bcl_eta_ext_init,
eps_in_min,
new_bcl_mu_in,
new_bcl_mu_eq,
new_bcl_mu_in_inv,
new_bcl_mu_eq_inv);
} else {
Martinez_update(qpsettings,
qpresults,
primal_feasibility_lhs_new,
primal_feasibility_lhs,
bcl_eta_in,
eps_in_min,
new_bcl_mu_in,
new_bcl_mu_eq,
new_bcl_mu_in_inv,
new_bcl_mu_eq_inv);
}
// 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)) {
/* to put in debuger mode
if (qpsettings.verbose) {
std::cout << "cold restart" << std::endl;
}
*/
new_bcl_mu_in = qpsettings.cold_reset_mu_in;
new_bcl_mu_eq = qpsettings.cold_reset_mu_eq;
new_bcl_mu_in_inv = qpsettings.cold_reset_mu_in_inv;
new_bcl_mu_eq_inv = qpsettings.cold_reset_mu_eq_inv;
}
if (qpresults.info.mu_in != new_bcl_mu_in ||
qpresults.info.mu_eq != new_bcl_mu_eq) {
{
++qpresults.info.mu_updates;
}
mu_update(qpmodel,
qpresults,
qpwork,
n_constraints,
dense_backend,
new_bcl_mu_eq,
new_bcl_mu_in);
}
qpresults.info.mu_eq = new_bcl_mu_eq;
qpresults.info.mu_in = new_bcl_mu_in;
qpresults.info.mu_eq_inv = new_bcl_mu_eq_inv;
qpresults.info.mu_in_inv = new_bcl_mu_in_inv;
}
ruiz.unscale_primal_in_place(VectorViewMut<T>{ from_eigen, qpresults.x });
ruiz.unscale_dual_in_place_eq(VectorViewMut<T>{ from_eigen, qpresults.y });
ruiz.unscale_dual_in_place_in(
VectorViewMut<T>{ from_eigen, qpresults.z.head(qpmodel.n_in) });
if (box_constraints) {
ruiz.unscale_box_dual_in_place_in(
VectorViewMut<T>{ 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<T>{ from_eigen, qpresults.se });
ruiz.unscale_primal_residual_in_place_in(
VectorViewMut<T>{ from_eigen, qpresults.si.head(qpmodel.n_in) });
if (box_constraints) {
ruiz.unscale_box_primal_residual_in_place_in(
VectorViewMut<T>{ 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);
}
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;
}
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;
}
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_EIGEN_MALLOC_ALLOWED();
}
} // namespace dense
} // namespace proxqp
} // namespace proxsuite
#endif /* end of include guard PROXSUITE_PROXQP_DENSE_SOLVER_HPP */