"""
OSQP Solver pure python implementation: low level module
"""
from __future__ import print_function
from builtins import range
from builtins import object
import numpy as np
import scipy as sp
import scipy.sparse as spspa
import scipy.sparse.linalg as spla
import numpy.linalg as la
import time # Time execution
# Solver Constants
OSQP_DUAL_INFEASIBLE_INACCURATE = 4
OSQP_PRIMAL_INFEASIBLE_INACCURATE = 3
OSQP_SOLVED_INACCURATE = 2
OSQP_SOLVED = 1
OSQP_MAX_ITER_REACHED = -2
OSQP_PRIMAL_INFEASIBLE = -3
OSQP_DUAL_INFEASIBLE = -4
OSQP_NON_CVX = -7
OSQP_UNSOLVED = -10
# Parameter bounds
RHO_MIN = 1e-06
RHO_MAX = 1e+06
RHO_EQ_OVER_RHO_INEQ = 1e+03
RHO_TOL = 1e-04
# Printing interval
PRINT_INTERVAL = 200
# OSQP Infinity
OSQP_INFTY = 1e+30
# OSQP Nan
OSQP_NAN = np.nan
# Linear system solver options
QDLDL_SOLVER = 0
# Scaling
MIN_SCALING = 1e-04
MAX_SCALING = 1e+04
class workspace(object):
"""
OSQP solver workspace
Attributes
----------
data - scaled QP problem
info - solver information
linsys_solver - structure for linear system solution
scaling - scaling matrices
settings - settings structure
solution - solution structure
Additional workspace variables
------------------------------
first_run - flag to indicate if it is the first run
clear_update_time - flag to indicate if update_time should be cleared
timer - saved time instant for timing purposes
x - primal iterate
x_prev - previous primal iterate
xz_tilde - x_tilde and z_tilde iterates stacked together
y - dual iterate
z - z iterate
z_prev - previous z iterate
Vectorized rho parameter
------------------------
rho_vec - vector of rho values for each constraint
rho_inv_vec - vector of reciprocal rho values
constr_type - type of constraints: loose (-1), eq (1), ineq (0)
Primal infeasibility related workspace variables
------------------------------------------------
delta_y - difference of consecutive y
Atdelta_y - A' * delta_y
Dual infeasibility related workspace variables
----------------------------------------------
delta_x - difference of consecutive x
Pdelta_x - P * delta_x
Adelta_x - A * delta_x
"""
class problem(object):
"""
QP problem of the form
minimize 1/2 x' P x + q' x
subject to l <= A x <= u
Attributes
----------
P, q
A, l, u
"""
def __init__(self, dims, Pdata, Pindices, Pindptr, q,
Adata, Aindices, Aindptr,
l, u):
# Set problem dimensions
(self.n, self.m) = dims
# Set problem data
self.P = spspa.csc_matrix((Pdata, Pindices, Pindptr),
shape=(self.n, self.n))
self.q = q
self.A = spspa.csc_matrix((Adata, Aindices, Aindptr),
shape=(self.m, self.n))
self.l = l if l is not None else -np.inf*np.ones(self.m)
self.u = u if u is not None else np.inf*np.ones(self.m)
class settings(object):
"""
OSQP solver settings
Attributes
----------
-> These cannot be changed without running setup
sigma [1e-06] - Regularization parameter for polish
scaling [10] - Scaling/Equilibration iterations (0 disabled)
-> These can be changed without running setup
rho [1.6] - Step in ADMM procedure
max_iter [4000] - Maximum number of iterations
eps_abs [1e-05] - Absolute tolerance
eps_rel [1e-05] - Relative tolerance
eps_prim_inf [1e-06] - Primal infeasibility tolerance
eps_dual_inf [1e-06] - Dual infeasibility tolerance
alpha [1.6] - Relaxation parameter
delta [1.0] - Regularization parameter for polish
verbose [True] - Verbosity
scaled_termination [False] - Evalute scaled termination criteria
check_termination [True] - Interval for termination checking
warm_start [False] - Reuse solution from previous solve
polish [False] - Solution polish
polish_refine_iter [3] - Iterative refinement iterations
"""
def __init__(self, **kwargs):
self.rho = kwargs.pop('rho', 0.1)
self.sigma = kwargs.pop('sigma', 1e-06)
self.scaling = kwargs.pop('scaling', 10)
self.max_iter = kwargs.pop('max_iter', 4000)
self.eps_abs = kwargs.pop('eps_abs', 1e-3)
self.eps_rel = kwargs.pop('eps_rel', 1e-3)
self.eps_prim_inf = kwargs.pop('eps_prim_inf', 1e-4)
self.eps_dual_inf = kwargs.pop('eps_dual_inf', 1e-4)
self.alpha = kwargs.pop('alpha', 1.6)
self.linsys_solver = kwargs.pop('linsys_solver', QDLDL_SOLVER)
self.delta = kwargs.pop('delta', 1e-6)
self.verbose = kwargs.pop('verbose', True)
self.scaled_termination = kwargs.pop('scaled_termination', False)
self.check_termination = kwargs.pop('check_termination', True)
self.warm_start = kwargs.pop('warm_start', True)
self.polish = kwargs.pop('polish', False)
self.polish_refine_iter = kwargs.pop('polish_refine_iter', 3)
self.adaptive_rho = kwargs.pop('adaptive_rho', True)
self.adaptive_rho_interval = kwargs.pop('adaptive_rho_interval', 200)
self.adaptive_rho_tolerance = kwargs.pop('adaptive_rho_tolerance', 5)
self.adaptive_rho_fraction = kwargs.pop('adaptive_rho_fraction', 0.7)
class scaling(object):
"""
Matrices for diagonal scaling
Attributes
----------
D - matrix in R^{n \\times n}
E - matrix in R^{m \\times n}
Dinv - inverse of D
Einv - inverse of E
c - cost scaling
cinv - inverse of cost scaling
"""
def __init__(self):
self.D = None
self.E = None
self.Dinv = None
self.Einv = None
self.c = None
self.cinv = None
class linesearch(object):
"""
Vectors obtained from line search between the ADMM and the polished
solution
Attributes
----------
X - matrix in R^{N \\times n}
Z - matrix in R^{N \\times m}
Y - matrix in R^{N \\times m}
t - vector in R^N
"""
def __init__(self):
self.X = None
self.Z = None
self.Y = None
self.t = None
class solution(object):
"""
Solver solution vectors z, u
"""
def __init__(self):
self.x = None
self.y = None
class info(object):
"""
Solver information
Attributes
----------
iter - number of iterations taken
status - status string, e.g. 'Solved'
status_val - status as c_int, defined in constants.h
status_polish - polish status: successful (1), not (0)
obj_val - primal objective
pri_res - norm of primal residual
dua_res - norm of dual residual
setup_time - time taken for setup phase (seconds)
solve_time - time taken for solve phase (seconds)
update_time - time taken for update phase (seconds)
polish_time - time taken for polish phase (seconds)
run_time - total time (seconds)
rho_updates - number of rho updates
rho_estimate - optimal rho estimate
"""
def __init__(self):
self.iter = 0
self.status_val = OSQP_UNSOLVED
self.status = "Unsolved"
self.status_polish = 0
self.update_time = 0.0
self.polish_time = 0.0
self.rho_updates = 0.0
class polish(object):
"""
Polishing structure containing active constraints at the solution
Attributes
----------
ind_low - indices of lower-active constraints
ind_upp - indices of upper-active constraints
n_low - number of lower-active constraints
n_upp - number of upper-active constraints
Ared - Part of A containing only active rows
x - polished x
z - polished z
y - polished y
"""
def __init__(self):
self.ind_low = None
self.ind_upp = None
self.n_low = None
self.n_upp = None
self.Ared = None
self.x = None
self.z = None
self.y = None
class linsys_solver(object):
"""
Linear systems solver
"""
def __init__(self, work):
"""
Initialize structure for KKT system solution
"""
# Construct reduced KKT matrix
KKT = spspa.vstack([
spspa.hstack([work.data.P + work.settings.sigma *
spspa.eye(work.data.n), work.data.A.T]),
spspa.hstack([work.data.A, -spspa.diags(work.rho_inv_vec)])])
# Initialize structure
self.kkt_factor = spla.splu(KKT.tocsc())
# self.lu, self.piv = sp.linalg.lu_factor(KKT.todense())
def solve(self, rhs):
"""
Solve linear system with given factorization
"""
return self.kkt_factor.solve(rhs)
# return sp.linalg.lu_solve((self.lu, self.piv), rhs)
class results(object):
"""
Results structure
Attributes
----------
x - primal solution
y - dual solution
info - info structure
"""
def __init__(self, solution, info, linesearch):
self.x = solution.x
self.y = solution.y
self.info = info
self.linesearch = linesearch
class OSQP(object):
"""OSQP solver lower level interface
Attributes
----------
work - workspace
"""
def __init__(self):
self._version = "0.6.1"
@property
def version(self):
"""Return solver version
"""
return self._version
def _norm_KKT_cols(self, P, A):
"""
Compute the norm of the KKT matrix from P and A
"""
# First half
norm_P_cols = spspa.linalg.norm(P, np.inf, axis=0)
norm_A_cols = spspa.linalg.norm(A, np.inf, axis=0)
norm_first_half = np.maximum(norm_P_cols, norm_A_cols)
# Second half (norm cols of A')
norm_second_half = spspa.linalg.norm(A, np.inf, axis=1)
return np.hstack((norm_first_half, norm_second_half))
def _limit_scaling(self, norm_vec):
"""
Norm vector for scaling
"""
if isinstance(norm_vec, (list, tuple, np.ndarray)): # Array
n = len(norm_vec)
new_norm_vec = np.zeros(n)
for i in range(n):
if norm_vec[i] < MIN_SCALING:
new_norm_vec[i] = 1.
elif norm_vec[i] > MAX_SCALING:
new_norm_vec[i] = MAX_SCALING
else:
new_norm_vec[i] = norm_vec[i]
else: # Scalar
if norm_vec < MIN_SCALING:
new_norm_vec = 1.
elif norm_vec > MAX_SCALING:
new_norm_vec = MAX_SCALING
else:
new_norm_vec = norm_vec
return new_norm_vec
def scale_data(self):
"""
Perform symmetric diagonal scaling via equilibration
"""
n = self.work.data.n
m = self.work.data.m
# Initialize scaling
s_temp = np.ones(n + m)
c = 1.0 # Cost scaling
# Define data
P = self.work.data.P
q = self.work.data.q
A = self.work.data.A
l = self.work.data.l
u = self.work.data.u
# Initialize scaler matrices
D = spspa.eye(n)
if m == 0:
# spspa.diags() will throw an error if fed with an empty array
E = spspa.csc_matrix((0, 0))
else:
E = spspa.eye(m)
# Iterate Scaling
for i in range(self.work.settings.scaling):
# First Step Ruiz
norm_cols = self._norm_KKT_cols(P, A)
norm_cols = self._limit_scaling(norm_cols) # Limit scaling
sqrt_norm_cols = np.sqrt(norm_cols) # Compute sqrt
s_temp = np.reciprocal(sqrt_norm_cols) # Elementwise recipr
# Obtain Scaler Matrices
D_temp = spspa.diags(s_temp[:self.work.data.n])
if m == 0:
# spspa.diags() will throw an error if fed with an empty array
E_temp = spspa.csc_matrix((0, 0))
else:
E_temp = spspa.diags(s_temp[self.work.data.n:])
# Scale data in place
P = D_temp.dot(P.dot(D_temp)).tocsc()
A = E_temp.dot(A.dot(D_temp)).tocsc()
q = D_temp.dot(q)
l = E_temp.dot(l)
u = E_temp.dot(u)
# Update equilibration matrices D and E
D = D_temp.dot(D)
E = E_temp.dot(E)
# Second Step cost normalization
norm_P_cols = spla.norm(P, np.inf, axis=0).mean()
inf_norm_q = np.linalg.norm(q, np.inf)
inf_norm_q = self._limit_scaling(inf_norm_q)
scale_cost = np.maximum(inf_norm_q, norm_P_cols)
scale_cost = self._limit_scaling(scale_cost)
scale_cost = 1. / scale_cost
# scale_cost = 1. / np.maximum(np.minimum(
# scale_cost, MAX_SCALING), MIN_SCALING)
# print("trace P", P.todense().trace()[0, 0])
# print("sum_norm_P_cols", spla.norm(P, np.inf, axis=0).sum())
# print("norm_P_cols", norm_P_cols)
# print("inf_norm_q", inf_norm_q)
# print("Scale cost = %.2e" % scale_cost)
# norm_cost = self._limit_scaling(norm_cost)
c_temp = scale_cost
# c_temp = 1.0
# Normalize cost
P = c_temp * P
q = c_temp * q
# Update scaling
c = c_temp * c
if self.work.settings.verbose:
print("Final cost scaling = %.10f" % c)
# Assign scaled problem
self.work.data = problem((n, m), P.data, P.indices, P.indptr, q,
A.data, A.indices, A.indptr, l, u)
# Assign scaling matrices
self.work.scaling = scaling()
self.work.scaling.D = D
self.work.scaling.Dinv = \
spspa.diags(np.reciprocal(D.diagonal()))
self.work.scaling.E = E
if m == 0:
self.work.scaling.Einv = E
else:
self.work.scaling.Einv = \
spspa.diags(np.reciprocal(E.diagonal()))
self.work.scaling.c = c
self.work.scaling.cinv = 1. / c
def set_rho_vec(self):
"""
Set values of rho vector based on constraint types
"""
self.work.settings.rho = np.minimum(np.maximum(self.work.settings.rho,
RHO_MIN), RHO_MAX)
# Find indices of loose bounds, equality constr and one-sided constr
loose_ind = np.where(np.logical_and(
self.work.data.l < -OSQP_INFTY*MIN_SCALING,
self.work.data.u > OSQP_INFTY*MIN_SCALING))[0]
eq_ind = np.where(self.work.data.u - self.work.data.l < RHO_TOL)[0]
ineq_ind = np.setdiff1d(np.setdiff1d(np.arange(self.work.data.m),
loose_ind), eq_ind)
# Type of constraints
self.work.constr_type[loose_ind] = -1
self.work.constr_type[eq_ind] = 1
self.work.constr_type[ineq_ind] = 0
self.work.rho_vec[loose_ind] = RHO_MIN
self.work.rho_vec[eq_ind] = RHO_EQ_OVER_RHO_INEQ * \
self.work.settings.rho
self.work.rho_vec[ineq_ind] = self.work.settings.rho
self.work.rho_inv_vec = np.reciprocal(self.work.rho_vec)
def update_rho_vec(self):
"""
Update values of rho_vec and refactor if constraints change.
"""
# Find indices of loose bounds, equality constr and one-sided constr
loose_ind = np.where(np.logical_and(
self.work.data.l < -OSQP_INFTY*MIN_SCALING,
self.work.data.u > OSQP_INFTY*MIN_SCALING))[0]
eq_ind = np.where(self.work.data.u - self.work.data.l < RHO_TOL)[0]
ineq_ind = np.setdiff1d(np.setdiff1d(np.arange(self.work.data.m),
loose_ind), eq_ind)
# Find indices of current constraint types
old_loose_ind = np.where(self.work.constr_type == -1)
old_eq_ind = np.where(self.work.constr_type == 1)
old_ineq_ind = np.where(self.work.constr_type == 0)
# Check if type of any constraint changed
constr_type_changed = (loose_ind != old_loose_ind).any() or \
(eq_ind != old_eq_ind).any() or \
(ineq_ind != old_ineq_ind).any()
# Update type of constraints
self.work.constr_type[loose_ind] = -1
self.work.constr_type[eq_ind] = 1
self.work.constr_type[ineq_ind] = 0
self.work.rho_vec[loose_ind] = RHO_MIN
self.work.rho_vec[eq_ind] = RHO_EQ_OVER_RHO_INEQ * \
self.work.settings.rho
self.work.rho_vec[ineq_ind] = self.work.settings.rho
self.work.rho_inv_vec = np.reciprocal(self.work.rho_vec)
if constr_type_changed:
self.work.linsys_solver = linsys_solver(self.work)
def print_setup_header(self, data, settings):
"""Print solver header
"""
print("--------------------------------------------------------------")
print(" OSQP v%s - Operator Splitting QP Solver" %
self.version)
print(" Pure Python Implementation")
print(" (c) Bartolomeo Stellato, Goran Banjac")
print(" University of Oxford - Stanford University 2017")
print("--------------------------------------------------------------")
print("problem: variables n = %d, constraints m = %d" %
(data.n, data.m))
nnz = self.work.data.P.nnz + self.work.data.A.nnz
print(" nnz(P) + nnz(A) = %i" % nnz)
print("settings: ", end='')
if settings.linsys_solver == QDLDL_SOLVER:
print("linear system solver = qdldl\n ", end='')
print("eps_abs = %.2e, eps_rel = %.2e," %
(settings.eps_abs, settings.eps_rel))
print(" eps_prim_inf = %.2e, eps_dual_inf = %.2e," %
(settings.eps_prim_inf, settings.eps_dual_inf))
print(" rho = %.2e " % settings.rho, end='')
if settings.adaptive_rho:
print("(adaptive)")
else:
print("")
print(" sigma = %.2e, alpha = %.2f, " %
(settings.sigma, settings.alpha), end='')
print("max_iter = %d" % settings.max_iter)
if settings.scaling:
print(" scaling: on, ", end='')
else:
print(" scaling: off, ", end='')
if settings.scaled_termination:
print("scaled_termination: on")
else:
print("scaled_termination: off")
if settings.warm_start:
print(" warm_start: on, ", end='')
else:
print(" warm_start: off, ", end='')
if settings.polish:
print("polish: on")
else:
print("polish: off")
print("")
def print_header(self):
"""
Print header before the iterations
"""
print("iter objective pri res dua res rho time")
def update_status(self, status):
self.work.info.status_val = status
if status == OSQP_SOLVED:
self.work.info.status = "solved"
if status == OSQP_SOLVED_INACCURATE:
self.work.info.status = "solved inaccurate"
elif status == OSQP_PRIMAL_INFEASIBLE:
self.work.info.status = "primal infeasible"
elif status == OSQP_PRIMAL_INFEASIBLE_INACCURATE:
self.work.info.status = "primal infeasible inaccurate"
elif status == OSQP_UNSOLVED:
self.work.info.status = "unsolved"
elif status == OSQP_DUAL_INFEASIBLE:
self.work.info.status = "dual infeasible"
elif status == OSQP_DUAL_INFEASIBLE_INACCURATE:
self.work.info.status = "dual infeasible inaccurate"
elif status == OSQP_MAX_ITER_REACHED:
self.work.info.status = "maximum iterations reached"
elif status == OSQP_NON_CVX:
self.work.info.status = "problem non convex"
def cold_start(self):
"""
Cold start optimization variables to zero
"""
self.work.x = np.zeros(self.work.data.n)
self.work.z = np.zeros(self.work.data.m)
self.work.y = np.zeros(self.work.data.m)
def update_xz_tilde(self):
"""
First ADMM step: update xz_tilde
"""
# Compute rhs and store it in xz_tilde
self.work.xz_tilde[:self.work.data.n] = \
self.work.settings.sigma * self.work.x_prev - self.work.data.q
self.work.xz_tilde[self.work.data.n:] = \
self.work.z_prev - self.work.rho_inv_vec * self.work.y
# Solve linear system
self.work.xz_tilde = self.work.linsys_solver.solve(self.work.xz_tilde)
# Update z_tilde
self.work.xz_tilde[self.work.data.n:] = \
self.work.z_prev + self.work.rho_inv_vec * \
(self.work.xz_tilde[self.work.data.n:] - self.work.y)
def update_x(self):
"""
Update x variable in second ADMM step
"""
self.work.x = \
self.work.settings.alpha * self.work.xz_tilde[:self.work.data.n] +\
(1. - self.work.settings.alpha) * self.work.x_prev
self.work.delta_x = self.work.x - self.work.x_prev
def project(self, z):
"""
Project z variable in set C (for now C = [l, u])
"""
return np.minimum(np.maximum(z, self.work.data.l), self.work.data.u)
def project_normalcone(self, z, y):
tmp = z + y
z = np.minimum(np.maximum(tmp, self.work.data.l), self.work.data.u)
y = tmp - z
return z, y
def update_z(self):
"""
Update z variable in second ADMM step
"""
self.work.z = \
self.work.settings.alpha * self.work.xz_tilde[self.work.data.n:] +\
(1. - self.work.settings.alpha) * self.work.z_prev +\
self.work.rho_inv_vec * self.work.y
self.work.z = self.project(self.work.z)
def update_y(self):
"""
Third ADMM step: update dual variable y
"""
self.work.delta_y = self.work.rho_vec * \
(self.work.settings.alpha * self.work.xz_tilde[self.work.data.n:] +
(1. - self.work.settings.alpha) * self.work.z_prev -
self.work.z)
self.work.y += self.work.delta_y
def compute_obj_val(self, x):
# Compute quadratic objective value for the given x
obj_val = .5 * np.dot(x, self.work.data.P.dot(x)) + \
np.dot(self.work.data.q, x)
if self.work.settings.scaling:
obj_val *= self.work.scaling.cinv
return obj_val
def compute_pri_res(self, x, z):
"""
Compute primal residual ||Ax - z||
"""
# Primal residual
Ax = self.work.data.A.dot(x)
pri_res = Ax - z
if self.work.settings.scaling and not \
self.work.settings.scaled_termination:
pri_res = self.work.scaling.Einv.dot(pri_res)
return la.norm(pri_res, np.inf)
def compute_pri_tol(self, eps_abs, eps_rel):
"""
Compute primal tolerance using problem data
"""
A = self.work.data.A
if self.work.settings.scaling and not \
self.work.settings.scaled_termination:
Einv = self.work.scaling.Einv
max_rel_eps = np.max([
la.norm(Einv.dot(A.dot(self.work.x)), np.inf),
la.norm(Einv.dot(self.work.z), np.inf)])
else:
max_rel_eps = np.max([
la.norm(A.dot(self.work.x), np.inf),
la.norm(self.work.z, np.inf)])
eps_pri = eps_abs + eps_rel * max_rel_eps
return eps_pri
def compute_dua_res(self, x, y):
"""
Compute dual residual ||Px + q + A'y||
"""
dua_res = self.work.data.P.dot(x) +\
self.work.data.q + self.work.data.A.T.dot(y)
if self.work.settings.scaling and not \
self.work.settings.scaled_termination:
# Use unscaled residual
dua_res = self.work.scaling.cinv * \
self.work.scaling.Dinv.dot(dua_res)
return la.norm(dua_res, np.inf)
def compute_dua_tol(self, eps_abs, eps_rel):
"""
Compute dual tolerance
"""
P = self.work.data.P
q = self.work.data.q
A = self.work.data.A
if self.work.settings.scaling and not \
self.work.settings.scaled_termination:
cinv = self.work.scaling.cinv
Dinv = self.work.scaling.Dinv
max_rel_eps = cinv * np.max([
la.norm(Dinv.dot(A.T.dot(self.work.y)), np.inf),
la.norm(Dinv.dot(P.dot(self.work.x)), np.inf),
la.norm(Dinv.dot(q), np.inf)])
else:
max_rel_eps = np.max([
la.norm(A.T.dot(self.work.y), np.inf),
la.norm(P.dot(self.work.x), np.inf),
la.norm(q, np.inf)])
eps_dua = eps_abs + eps_rel * max_rel_eps
return eps_dua
def is_primal_infeasible(self, eps_prim_inf):
"""
Check primal infeasibility
||A'*v||_2 = 0
with v = delta_y/||delta_y||_2 given that following condition holds
u'*(v)_{+} + l'*(v)_{-} < 0
"""
# Rescale delta_y
if self.work.settings.scaling and not \
self.work.settings.scaled_termination:
norm_delta_y = la.norm(self.work.scaling.E.dot(self.work.delta_y),
np.inf)
else:
norm_delta_y = la.norm(self.work.delta_y, np.inf)
if norm_delta_y > eps_prim_inf:
lhs = self.work.data.u.dot(np.maximum(self.work.delta_y, 0)) + \
self.work.data.l.dot(np.minimum(self.work.delta_y, 0))
if lhs < -eps_prim_inf * norm_delta_y:
self.work.Atdelta_y = self.work.data.A.T.dot(self.work.delta_y)
if self.work.settings.scaling and not \
self.work.settings.scaled_termination:
self.work.Atdelta_y = \
self.work.scaling.Dinv.dot(self.work.Atdelta_y)
return la.norm(self.work.Atdelta_y, np.inf) < \
eps_prim_inf * norm_delta_y
return False
def is_dual_infeasible(self, eps_dual_inf):
"""
Check dual infeasibility
||P*v||_inf = 0
with v = delta_x / ||delta_x||_inf given that the following
conditions hold
q'* v < 0 and
| 0 if l_i, u_i \in R
(A * v)_i = { >= 0 if u_i = +inf
| <= 0 if l_i = -inf
"""
# Rescale delta_x
if self.work.settings.scaling and not \
self.work.settings.scaled_termination:
norm_delta_x = la.norm(self.work.scaling.D.dot(self.work.delta_x),
np.inf)
scale_cost = self.work.scaling.c
else:
norm_delta_x = la.norm(self.work.delta_x, np.inf)
scale_cost = 1.0
# Prevent 0 division
if norm_delta_x > eps_dual_inf:
# First check q'* delta_x < 0
if self.work.data.q.dot(self.work.delta_x) < \
- scale_cost * eps_dual_inf * norm_delta_x:
# Compute P * delta_x
self.work.Pdelta_x = self.work.data.P.dot(self.work.delta_x)
# Scale if necessary
if self.work.settings.scaling and not \
self.work.settings.scaled_termination:
self.work.Pdelta_x = \
self.work.scaling.Dinv.dot(self.work.Pdelta_x)
# Check if ||P * delta_x|| = 0
if la.norm(self.work.Pdelta_x, np.inf) < \
scale_cost * eps_dual_inf * norm_delta_x:
# Compute A * delta_x
self.work.Adelta_x = self.work.data.A.dot(
self.work.delta_x)
# Scale if necessary
if self.work.settings.scaling and not \
self.work.settings.scaled_termination:
self.work.Adelta_x = \
self.work.scaling.Einv.dot(self.work.Adelta_x)
for i in range(self.work.data.m):
# De Morgan's Law applied to negate
# conditions on A * delta_x
if ((self.work.data.u[i] < OSQP_INFTY*MIN_SCALING) and
(self.work.Adelta_x[i] >
eps_dual_inf * norm_delta_x)) or \
((self.work.data.l[i] > -OSQP_INFTY*MIN_SCALING) and
(self.work.Adelta_x[i] <
-eps_dual_inf * norm_delta_x)):
# At least one condition not satisfied
return False
# All conditions passed -> dual infeasible
return True
# No all checks managed to pass. Problem not dual infeasible
return False
def compute_rho_estimate(self):
# Iterates
x = self.work.x
y = self.work.y
z = self.work.z
# Problem data
P = self.work.data.P
q = self.work.data.q
A = self.work.data.A
# Compute normalized residuals
pri_res = la.norm(A.dot(x) - z, np.inf)
pri_res /= (np.max([la.norm(A.dot(x), np.inf),
la.norm(z, np.inf)]) + 1e-10)
dua_res = la.norm(P.dot(x) + q + A.T.dot(y), np.inf)
dua_res /= (np.max([la.norm(A.T.dot(y), np.inf),
la.norm(P.dot(x), np.inf),
la.norm(q, np.inf)]) + 1e-10)
# Compute new rho
new_rho = self.work.settings.rho * np.sqrt(pri_res/(dua_res + 1e-10))
return min(max(new_rho, RHO_MIN), RHO_MAX)
def adapt_rho(self):
"""
Adapt rho value based on current primal and dual residuals
"""
# Compute new rho
rho_new = self.compute_rho_estimate()
# Update rho estimate
self.work.info.rho_estimate = rho_new
# Settings
adaptive_rho_tolerance = self.work.settings.adaptive_rho_tolerance
if rho_new > adaptive_rho_tolerance * self.work.settings.rho or \
rho_new < 1. / adaptive_rho_tolerance * \
self.work.settings.rho:
# Update rho
self.update_rho(rho_new)
# Update rho updates count
self.work.info.rho_updates += 1
def reset_info(self, info):
"""
Reset information after problem updates
"""
info.solve_time = 0.0
info.polish_time = 0.0
self.update_status(OSQP_UNSOLVED)
info.rho_updates = 0
def update_info(self, iter, polish):
"""
Update information at iterations
"""
if polish == 1:
self.work.pol.obj_val = self.compute_obj_val(self.work.pol.x)
self.work.pol.pri_res = self.compute_pri_res(self.work.pol.x,
self.work.pol.z)
self.work.pol.dua_res = self.compute_dua_res(self.work.pol.x,
self.work.pol.y)
self.work.info.polish_time = time.time() - self.work.timer
else:
self.work.info.iter = iter
self.work.info.obj_val = self.compute_obj_val(self.work.x)
self.work.info.pri_res = self.compute_pri_res(self.work.x,
self.work.z)
self.work.info.dua_res = self.compute_dua_res(self.work.x,
self.work.y)
self.work.info.solve_time = time.time() - self.work.timer
def print_summary(self):
"""
Print status summary at each ADMM iteration
"""
if self.work.first_run:
runtime = self.work.info.setup_time + self.work.info.solve_time
else:
runtime = self.work.info.update_time + self.work.info.solve_time
print("%4i %11.4e %8.2e %8.2e %8.2e %8.2es" %
(self.work.info.iter,
self.work.info.obj_val,
self.work.info.pri_res,
self.work.info.dua_res,
self.work.settings.rho,
runtime))
def print_polish(self):
"""
Print polish information
"""
if self.work.first_run:
runtime = self.work.info.setup_time + self.work.info.solve_time + \
self.work.info.polish_time
else:
runtime = self.work.info.update_time + self.work.info.solve_time + \
self.work.info.polish_time
print("plsh %11.4e %8.2e %8.2e -------- %8.2es" %
(self.work.info.obj_val,
self.work.info.pri_res,
self.work.info.dua_res,
runtime))
def check_termination(self, approximate=False):
"""
Check residuals for algorithm convergence and update solver status
Args
----
approximate: bool to determine if termination criteria are
approximate or accurate
"""
pri_check = 0
dua_check = 0
prim_inf_check = 0
dual_inf_check = 0
eps_abs = self.work.settings.eps_abs
eps_rel = self.work.settings.eps_rel
eps_prim_inf = self.work.settings.eps_prim_inf
eps_dual_inf = self.work.settings.eps_dual_inf
if approximate:
eps_abs *= 10
eps_rel *= 10
eps_prim_inf *= 10
eps_dual_inf *= 10
# If residuals are too large, the problem is probably non convex
if (self.work.info.pri_res > OSQP_INFTY) or (self.work.info.dua_res > OSQP_INFTY):
self.work.info.status_val = OSQP_NON_CVX
self.work.info.obj_val = OSQP_NAN
return 1
if self.work.data.m == 0: # No constraints -> always primal feasible
pri_check = 1
else:
# Compute primal tolerance
eps_pri = self.compute_pri_tol(eps_abs, eps_rel)
if self.work.info.pri_res < eps_pri:
pri_check = 1
else:
# Check infeasibility
prim_inf_check = self.is_primal_infeasible(eps_prim_inf)
# Compute dual tolerance
eps_dua = self.compute_dua_tol(eps_abs, eps_rel)
if self.work.info.dua_res < eps_dua:
dua_check = 1
else:
# Check dual infeasibility
dual_inf_check = self.is_dual_infeasible(eps_dual_inf)
# Compare residuals and determine solver status
if pri_check & dua_check:
if approximate:
self.work.info.status_val = OSQP_SOLVED_INACCURATE
else:
self.work.info.status_val = OSQP_SOLVED
return 1
elif prim_inf_check:
if approximate:
self.work.info.status_val = OSQP_PRIMAL_INFEASIBLE_INACCURATE
else:
self.work.info.status_val = OSQP_PRIMAL_INFEASIBLE
self.work.info.obj_val = OSQP_INFTY
# Store original certificate
if self.work.settings.scaling and not \
self.work.settings.scaled_termination:
self.work.delta_y = self.work.scaling.E.dot(self.work.delta_y)
return 1
elif dual_inf_check:
if approximate:
self.work.info.status_val = OSQP_DUAL_INFEASIBLE_INACCURATE
else:
self.work.info.status_val = OSQP_DUAL_INFEASIBLE
# Store original certificate
if self.work.settings.scaling and not \
self.work.settings.scaled_termination:
self.work.delta_x = self.work.scaling.D.dot(self.work.delta_x)
self.work.info.obj_val = -OSQP_INFTY
return 1
def print_footer(self):
"""
Print footer at the end of the optimization
"""
print("") # Add space after iterations
print("status: %s" % self.work.info.status)
if self.work.settings.polish and \
self.work.info.status_val == OSQP_SOLVED:
if self.work.info.status_polish == 1:
print("solution polish: successful")
elif self.work.info.status_polish == -1:
print("solution polish: unsuccessful")
print("number of iterations: %d" % self.work.info.iter)
if self.work.info.status_val == OSQP_SOLVED or \
self.work.info.status_val == OSQP_SOLVED_INACCURATE:
print("optimal objective: %.4f" % self.work.info.obj_val)
print("run time: %.2es" % (self.work.info.run_time))
print("optimal rho estimate: %.2es" %
(self.work.info.rho_estimate))
print("") # Print last space
def store_solution(self):
"""
Store current primal and dual solution in solution structure
"""
if (self.work.info.status_val is not OSQP_PRIMAL_INFEASIBLE) and \
(self.work.info.status_val is not OSQP_DUAL_INFEASIBLE):
self.work.solution.x = self.work.x
self.work.solution.y = self.work.y
# Unscale solution
if self.work.settings.scaling:
self.work.solution.x = \
self.work.scaling.D.dot(self.work.solution.x)
self.work.solution.y = \
self.work.scaling.cinv * \
self.work.scaling.E.dot(self.work.solution.y)
else:
self.work.solution.x = np.array([None] * self.work.data.n)
self.work.solution.y = np.array([None] * self.work.data.m)
#
# Main Solver API
#
def setup(self, dims, Pdata, Pindices, Pindptr, q,
Adata, Aindices, Aindptr,
l, u, **stgs):
"""
Perform OSQP solver setup QP problem of the form
minimize 1/2 x' P x + q' x
subject to l <= A x <= u
"""
(n, m) = dims
self.work = workspace()
# Start timer
self.work.timer = time.time()
# Unscaled problem data
self.work.data = problem((n, m), Pdata, Pindices, Pindptr, q,
Adata, Aindices, Aindptr,
l, u)
# Vectorized rho parameter
self.work.rho_vec = np.zeros(m)
self.work.rho_inv_vec = np.zeros(m)
# Type of constraints
self.work.constr_type = np.zeros(m)
# Initialize workspace variables
self.work.x = np.zeros(n)
self.work.z = np.zeros(m)
self.work.xz_tilde = np.zeros(n + m)
self.work.x_prev = np.zeros(n)
self.work.z_prev = np.zeros(m)
self.work.y = np.zeros(m)
self.work.delta_y = np.zeros(m) # Delta_y for primal infeasibility
# Flag indicating first run
self.work.first_run = 1
# Flag indicating that the update_time should be set to zero
self.work.clear_update_time = 0
# Settings
self.work.settings = settings(**stgs)
# Scale problem
if self.work.settings.scaling:
self.scale_data()
# Set type of constraints
self.set_rho_vec()
# Factorize KKT
self.work.linsys_solver = linsys_solver(self.work)
# Solution
self.work.solution = solution()
# Info
self.work.info = info()
# Polishing structure
self.work.pol = polish()
# End timer
self.work.info.setup_time = time.time() - self.work.timer
# Print setup header
if self.work.settings.verbose:
self.print_setup_header(self.work.data, self.work.settings)
def solve(self):
"""
Solve QP problem using OSQP
"""
# Start timer
self.work.timer = time.time()
# Clear update_time
if self.work.clear_update_time == 1:
self.work.info.update_time = 0.0
# Print header
if self.work.settings.verbose:
self.print_header()
# Cold start if not warm start
if not self.work.settings.warm_start:
self.cold_start()
# ADMM algorithm
for iter in range(1, self.work.settings.max_iter + 1):
# Update x_prev, z_prev
self.work.x_prev = np.copy(self.work.x)
self.work.z_prev = np.copy(self.work.z)
# Admm steps
# First step: update \tilde{x} and \tilde{z}
self.update_xz_tilde()
# Second step: update x and z
self.update_x()
self.update_z()
# Third step: update y
self.update_y()
if self.work.settings.check_termination:
# Update info
self.update_info(iter, 0)
# Print summary
if (self.work.settings.verbose) & \
((iter % PRINT_INTERVAL == 0) | (iter == 1)):
self.print_summary()
# Break if converged
if self.check_termination():
break
# If not terminated, update rho in case
if self.work.settings.adaptive_rho_interval and \
(iter % self.work.settings.adaptive_rho_interval == 0) \
and self.work.settings.adaptive_rho:
self.adapt_rho()
# DEBUG: Print
# if self.work.settings.verbose:
# print("rho = %.2e" % self.work.settings.rho)
if not self.work.settings.check_termination:
# Update info
self.update_info(self.work.settings.max_iter, 0)
# Print summary
if (self.work.settings.verbose):
self.print_summary()
# Break if converged
self.check_termination()
# Print summary for last iteration
if (self.work.settings.verbose) & (iter % PRINT_INTERVAL != 0):
self.print_summary()
# If max iterations reached, update status accordingly
if iter == self.work.settings.max_iter:
if not self.check_termination(approximate=True):
self.work.info.status_val = OSQP_MAX_ITER_REACHED
# Update status string
self.update_status(self.work.info.status_val)
# Update solve time
self.work.info.solve_time = time.time() - self.work.timer
# Update rho estimate
self.work.info.rho_estimate = self.compute_rho_estimate()
# Solution polish
if self.work.settings.polish and \
self.work.info.status_val == OSQP_SOLVED:
ls = self.polish()
else:
ls = None
# Update total times
if self.work.first_run:
self.work.info.run_time = self.work.info.setup_time + \
self.work.info.solve_time + self.work.info.polish_time
else:
self.work.info.run_time = self.work.info.update_time + \
self.work.info.solve_time + self.work.info.polish_time
# Print footer
if self.work.settings.verbose:
self.print_footer()
# Store solution
self.store_solution()
# Eliminate first run flag
if self.work.first_run:
self.work.first_run = 0
# Indicate that the update_time should be set to zero
self.work.clear_update_time = 1
# Store results structure
return results(self.work.solution, self.work.info, ls)
#
# Auxiliary API Functions
#
def update_lin_cost(self, q_new):
"""
Update linear cost without requiring factorization
"""
if self.work.clear_update_time == 1:
# Clear update_time
self.work.clear_update_time = 0
self.work.info.update_time = 0.0
# Start timer
self.work.timer = time.time()
# Copy cost vector
self.work.data.q = np.copy(q_new)
# Scaling
if self.work.settings.scaling:
self.work.data.q = self.work.scaling.c * \
self.work.scaling.D.dot(self.work.data.q)
# Reset solver info
self.reset_info(self.work.info)
# Update update_time
self.work.info.update_time += time.time() - self.work.timer
def update_bounds(self, l_new, u_new):
"""
Update counstraint bounds without requiring factorization
"""
if self.work.clear_update_time == 1:
# Clear update_time
self.work.clear_update_time = 0
self.work.info.update_time = 0.0
# Start timer
self.work.timer = time.time()
# Check if bounds are correct
if not np.greater_equal(u_new, l_new).all():
raise ValueError("Lower bound must be lower than" +
" or equal to upper bound!")
# Update vectors
self.work.data.l = np.copy(l_new)
self.work.data.u = np.copy(u_new)
# Scale vectors
if self.work.settings.scaling:
self.work.data.l = self.work.scaling.E.dot(self.work.data.l)
self.work.data.u = self.work.scaling.E.dot(self.work.data.u)
# Reset solver info
self.reset_info(self.work.info)
# If type of any constraint changed, update rho_vec and KKT matrix
self.update_rho_vec()
# Update update_time
self.work.info.update_time += time.time() - self.work.timer
def update_lower_bound(self, l_new):
"""
Update lower bound without requiring factorization
"""
if self.work.clear_update_time == 1:
# Clear update_time
self.work.clear_update_time = 0
self.work.info.update_time = 0.0
# Start timer
self.work.timer = time.time()
# Update lower bound
self.work.data.l = l_new
# Scale vector
if self.work.settings.scaling:
self.work.data.l = self.work.scaling.E.dot(self.work.data.l)
# Check values
if not np.greater_equal(self.work.data.u, self.work.data.l).all():
raise ValueError("Lower bound must be lower than" +
" or equal to upper bound!")
# Reset solver info
self.reset_info(self.work.info)
# If type of any constraint changed, update rho_vec and KKT matrix
self.update_rho_vec()
# Update update_time
self.work.info.update_time += time.time() - self.work.timer
def update_upper_bound(self, u_new):
"""
Update upper bound without requiring factorization
"""
if self.work.clear_update_time == 1:
# Clear update_time
self.work.clear_update_time = 0
self.work.info.update_time = 0.0
# Start timer
self.work.timer = time.time()
# Update upper bound
self.work.data.u = u_new
# Scale vector
if self.work.settings.scaling:
self.work.data.u = self.work.scaling.E.dot(self.work.data.u)
# Check values
if not np.greater_equal(self.work.data.u, self.work.data.l).all():
raise ValueError("Lower bound must be lower than" +
" or equal to upper bound!")
# Reset solver info
self.reset_info(self.work.info)
# If type of any constraint changed, update rho_vec and KKT matrix
self.update_rho_vec()
# Update update_time
self.work.info.update_time += time.time() - self.work.timer
def update_P(self, P_new):
"""
Update quadratic cost matrix
"""
if self.work.clear_update_time == 1:
# Clear update_time
self.work.clear_update_time = 0
self.work.info.update_time = 0.0
# Start timer
self.work.timer = time.time()
if self.work.settings.scaling:
self.work.data.P = \
self.work.scaling.c * \
self.work.scaling.D.dot(P_new.dot(self.work.scaling.D))
else:
self.work.data.P = P_new
self.work.linsys_solver = linsys_solver(self.work)
# Update update_time
self.work.info.update_time += time.time() - self.work.timer
def update_A(self, A_new):
"""
Update constraint matrix
"""
if self.work.clear_update_time == 1:
# Clear update_time
self.work.clear_update_time = 0
self.work.info.update_time = 0.0
# Start timer
self.work.timer = time.time()
if self.work.settings.scaling:
self.work.data.A = self.work.scaling.E.dot(A_new.dot(self.work.scaling.D))
else:
self.work.data.A = A_new
self.work.linsys_solver = linsys_solver(self.work)
# Update update_time
self.work.info.update_time += time.time() - self.work.timer
def update_P_A(self, P_new, A_new):
"""
Update quadratic cost and constraint matrices
"""
if self.work.clear_update_time == 1:
# Clear update_time
self.work.clear_update_time = 0
self.work.info.update_time = 0.0
# Start timer
self.work.timer = time.time()
if self.work.settings.scaling:
self.work.data.P = self.work.scaling.D.dot(P_new.dot(self.work.scaling.D))
self.work.data.A = self.work.scaling.E.dot(A_new.dot(self.work.scaling.D))
else:
self.work.data.P = P_new
self.work.data.A = A_new
self.work.linsys_solver = linsys_solver(self.work)
# Update update_time
self.work.info.update_time += time.time() - self.work.timer
def warm_start(self, x, y):
"""
Warm start primal and dual variables
"""
# Update warm_start setting to true
self.work.settings.warm_start = True
# Copy primal and dual variables into the iterates
self.work.x = x
self.work.y = y
# Scale iterates
self.work.x = self.work.scaling.Dinv.dot(self.work.x)
self.work.y = self.work.scaling.Einv.dot(self.work.y)
# Update z iterate as well
self.work.z = self.work.data.A.dot(self.work.x)
def warm_start_x(self, x):
"""
Warm start primal variable
"""
# Update warm_start setting to true
self.work.settings.warm_start = True
# Copy primal and dual variables into the iterates
self.work.x = x
# Scale iterates
self.work.x = self.work.scaling.Dinv.dot(self.work.x)
# Update z iterate as well
self.work.z = self.work.data.A.dot(self.work.x)
# Cold start y
self.work.y = np.zeros(self.work.data.m)
def warm_start_y(self, y):
"""
Warm start dual variable
"""
# Update warm_start setting to true
self.work.settings.warm_start = True
# Copy primal and dual variables into the iterates
self.work.y = y
# Scale iterates
self.work.y = self.work.scaling.Einv.dot(self.work.y)
# Cold start x and z
self.work.x = np.zeros(self.work.data.n)
self.work.z = np.zeros(self.work.data.m)
#
# Update Problem Settings
#
def update_max_iter(self, max_iter_new):
"""
Update maximum number of iterations
"""
# Check that maxiter is positive
if max_iter_new <= 0:
raise ValueError("max_iter must be positive")
# Update max_iter
self.work.settings.max_iter = max_iter_new
def update_eps_abs(self, eps_abs_new):
"""
Update absolute tolerance
"""
if eps_abs_new <= 0:
raise ValueError("eps_abs must be positive")
self.work.settings.eps_abs = eps_abs_new
def update_eps_rel(self, eps_rel_new):
"""
Update relative tolerance
"""
if eps_rel_new <= 0:
raise ValueError("eps_rel must be positive")
self.work.settings.eps_rel = eps_rel_new
def update_rho(self, rho_new):
"""
Update set-size parameter rho
"""
if rho_new <= 0:
raise ValueError("rho must be positive")
# Update rho
self.work.settings.rho = np.minimum(np.maximum(rho_new,
RHO_MIN), RHO_MAX)
# Update rho_vec and rho_inv_vec
ineq_ind = np.where(self.work.constr_type == 0)
eq_ind = np.where(self.work.constr_type == 1)
self.work.rho_vec[ineq_ind] = self.work.settings.rho
self.work.rho_vec[eq_ind] = RHO_EQ_OVER_RHO_INEQ * self.work.settings.rho
self.work.rho_inv_vec = np.reciprocal(self.work.rho_vec)
# Factorize KKT
self.work.linsys_solver = linsys_solver(self.work)
def update_alpha(self, alpha_new):
"""
Update relaxation parameter alpga
"""
if not (alpha_new >= 0 | alpha_new <= 2):
raise ValueError("alpha must be between 0 and 2")
self.work.settings.alpha = alpha_new
def update_delta(self, delta_new):
"""
Update delta parameter for polish
"""
if delta_new <= 0:
raise ValueError("delta must be positive")
self.work.settings.delta = delta_new
def update_polish(self, polish_new):
"""
Update polish parameter
"""
if (polish_new is not True) & (polish_new is not False):
raise ValueError("polish should be either True or False")
self.work.settings.polish = polish_new
self.work.info.polish_time = 0.0
def update_polish_refine_iter(self, polish_refine_iter_new):
"""
Update number iterative refinement iterations in polish
"""
if polish_refine_iter_new < 0:
raise ValueError("polish_refine_iter must be nonnegative")
self.work.settings.polish_refine_iter = polish_refine_iter_new
def update_verbose(self, verbose_new):
"""
Update verbose parameter
"""
if (verbose_new is not True) & (verbose_new is not False):
raise ValueError("verbose should be either True or False")
self.work.settings.verbose = verbose_new
def update_scaled_termination(self, scaled_termination_new):
"""
Update scaled_termination parameter
"""
if (scaled_termination_new is not True) & (scaled_termination_new is not False):
raise ValueError("scaled_termination should be either True or False")
self.work.settings.scaled_termination = scaled_termination_new
def update_check_termination(self, check_termination_new):
"""
Update check_termination parameter
"""
if check_termination_new <= 0:
raise ValueError("check_termination should be greater than 0")
self.work.settings.check_termination = check_termination_new
def update_warm_start(self, warm_start_new):
"""
Update warm_start parameter
"""
if (warm_start_new is not True) & (warm_start_new is not False):
raise ValueError("warm_start should be either True or False")
self.work.settings.warm_start = warm_start_new
def constant(self, constant_name):
"""
Return solver constant
"""
if constant_name == "OSQP_INFTY":
return OSQP_INFTY
if constant_name == "OSQP_NAN":
return OSQP_NAN
if constant_name == "OSQP_SOLVED":
return OSQP_SOLVED
if constant_name == "OSQP_UNSOLVED":
return OSQP_UNSOLVED
if constant_name == "OSQP_PRIMAL_INFEASIBLE":
return OSQP_PRIMAL_INFEASIBLE
if constant_name == "OSQP_DUAL_INFEASIBLE":
return OSQP_DUAL_INFEASIBLE
if constant_name == "OSQP_MAX_ITER_REACHED":
return OSQP_MAX_ITER_REACHED
raise ValueError('Constant not recognized!')
def iter_refin(self, KKT_factor, z, b):
"""
Iterative refinement of the solution of a linear system
1. (K + dK) * dz = b - K*z
2. z <- z + dz
"""
for i in range(self.work.settings.polish_refine_iter):
rhs = b - np.hstack([
self.work.data.P.dot(z[:self.work.data.n]) +
self.work.pol.Ared.T.dot(z[self.work.data.n:]),
self.work.pol.Ared.dot(z[:self.work.data.n])])
dz = KKT_factor.solve(rhs)
z += dz
return z
def polish(self):
"""
Solution polish:
Solve equality constrained QP with assumed active constraints.
"""
# Start timer
self.work.timer = time.time()
# Guess which linear constraints are lower-active, upper-active, free
self.work.pol.ind_low = np.where(self.work.z -
self.work.data.l < -self.work.y)[0]
self.work.pol.ind_upp = np.where(self.work.data.u -
self.work.z < self.work.y)[0]
self.work.pol.n_low = len(self.work.pol.ind_low)
self.work.pol.n_upp = len(self.work.pol.ind_upp)
# Form Ared from the assumed active constraints
self.work.pol.Ared = spspa.vstack([
self.work.data.A[self.work.pol.ind_low],
self.work.data.A[self.work.pol.ind_upp]])
# # Terminate if there are no active constraints
# if self.work.pol.Ared.shape[0] == 0:
# return
# Form and factorize reduced KKT
KKTred = spspa.vstack([
spspa.hstack([self.work.data.P + self.work.settings.delta *
spspa.eye(self.work.data.n),
self.work.pol.Ared.T]),
spspa.hstack([self.work.pol.Ared, -self.work.settings.delta *
spspa.eye(self.work.pol.Ared.shape[0])])])
KKTred_factor = spla.splu(KKTred.tocsc())
# Form reduced RHS
rhs_red = np.hstack([-self.work.data.q,
self.work.data.l[self.work.pol.ind_low],
self.work.data.u[self.work.pol.ind_upp]])
# Solve reduced KKT system
pol_sol = KKTred_factor.solve(rhs_red)
# Perform iterative refinement to compensate for the reg. error
if self.work.settings.polish_refine_iter > 0:
pol_sol = self.iter_refin(KKTred_factor, pol_sol, rhs_red)
# Store the polished solution (x,z,y)
self.work.pol.x = pol_sol[:self.work.data.n]
self.work.pol.z = self.work.data.A.dot(self.work.pol.x)
self.work.pol.y = np.zeros(self.work.data.m)
y_red = pol_sol[self.work.data.n:]
self.work.pol.y[self.work.pol.ind_low] = y_red[:self.work.pol.n_low]
self.work.pol.y[self.work.pol.ind_upp] = y_red[self.work.pol.n_low:]
# Ensure (z,y) satisfies normal cone constraint
self.work.pol.z, self.work.pol.y = \
self.project_normalcone(self.work.pol.z, self.work.pol.y)
# Compute primal and dual residuals of the polished solution
self.update_info(0, 1)
# Check if polish was successful
pol_success = (self.work.pol.pri_res < self.work.info.pri_res) and \
(self.work.pol.dua_res < self.work.info.dua_res) or \
(self.work.pol.pri_res < self.work.info.pri_res) and \
(self.work.info.dua_res < 1e-10) or \
(self.work.pol.dua_res < self.work.info.dua_res) and \
(self.work.info.pri_res < 1e-10)
ls = linesearch()
if pol_success:
# Update solver information
self.work.info.obj_val = self.work.pol.obj_val
self.work.info.pri_res = self.work.pol.pri_res
self.work.info.dua_res = self.work.pol.dua_res
self.work.info.status_polish = 1
# Update ADMM iterations
self.work.x = self.work.pol.x
self.work.z = self.work.pol.z
self.work.y = self.work.pol.y
# Print summary
if self.work.settings.verbose:
self.print_polish()
else:
self.work.info.status_polish = -1
# Line search on the line connecting the ADMM and the polished sol.
ls.t = np.linspace(0., 0.002, 1000)
ls.X, ls.Z, ls.Y = self.line_search(
self.work.x, self.work.z, self.work.y,
self.work.pol.x, self.work.pol.z, self.work.pol.y,
ls.t)
return ls
def line_search(self, x1, z1, y1, x2, z2, y2, t):
"""
Perform line search on the line between (x1,z1,y1) and (x2,z2,y2).
"""
N = len(t)
X = np.zeros((N, self.work.data.n))
Z = np.zeros((N, self.work.data.m))
Y = np.zeros((N, self.work.data.m))
dx = x2 - x1
dz = z2 - z1
dy = y2 - y1
for i in range(N):
X[i, :] = x1 + t[i] * dx
Z[i, :] = z1 + t[i] * dz
Y[i, :] = y1 + t[i] * dy
Z[i, :], Y[i, :] = self.project_normalcone(Z[i, :], Y[i, :])
# Unscale optimization variables (x,z,y)
if self.work.settings.scaling:
X[i, :] = self.work.scaling.D.dot(X[i, :])
Z[i, :] = self.work.scaling.Einv.dot(Z[i, :])
Y[i, :] = self.work.scaling.E.dot(Y[i, :])
return (X, Z, Y)
