Introduce AVIWorkspace to allow for hot starts

interfaces/daqp-julia/Project.toml

@@ -1,7 +1,7 @@
 name = "DAQPBase"
 uuid = "833476c3-a8c0-4073-9b64-6473509843fe"
 authors = ["Daniel Arnström <daniel.arnstrom@gmail.com>"]
-version = "0.3.4"
+version = "0.3.5"
 
 [deps]
 DAQP_jll = "5c51c916-43bf-57fe-9d62-13064ebbf40d"

interfaces/daqp-julia/src/avi.jl

@@ -1,3 +1,120 @@
+Base.@kwdef mutable struct AVISettings 
+    iter_limit::Int = 1000
+    inner_iter_limit::Int = 10000
+    primal_tol::Float64 = 1e-6
+    dual_tol::Float64 = 1e-12
+    alpha::Float64 = 1.0
+    beta::Float64 = 0.25
+end
+
+struct AVIWorkspace
+    H::Matrix{Float64}
+    f::Vector{Float64}
+    A::Matrix{Float64}
+    bu::Vector{Float64}
+    bl::Vector{Float64}
+
+    H_factor::LU{Float64}
+    x_unc::Vector{Float64}
+
+    H1pI::Matrix{Float64}
+    H2mI::Matrix{Float64}
+    H2pI::LU{Float64} # LU
+    H2::Matrix{Float64}
+    H2xpf::Vector{Float64}
+    x::Vector{Float64}
+    y::Vector{Float64}
+    daqp_workspace::DAQPBase.Model
+    kkt_buffer::Vector{Float64}
+
+    settings::AVISettings
+end
+
+AVIWorkspace() = AVIWorkspace(zeros(0,0),zeros(0),zeros(0,0),zeros(0),zeros(0), 
+                              lu(zeros(0,0)),zeros(0),
+                              zeros(0,0),zeros(0,0),lu(zeros(0,0)),zeros(0,0), zeros(0),
+                              zeros(0),zeros(0), Model(), zeros(0), AVISettings())
+
+
+function setup_avi(H,f,A,bu,bl,sense;x0 = zeros(0), daqp_workspace=nothing, settings=AVISettings())
+    n,m = length(f),length(bu)
+    bl = isempty(bl) ? fill(-1e30,m) : bl
+    sense = isempty(sense) ? zeros(Cint,m) : sense
+
+    H1 = (H+H') / 4
+    H2 = H1+(H-H') / 2
+    H1pI,H2mI,H2pI = H1+I, H2-I, lu!(H2+I)
+
+    #H_factor = lu(H)
+    H_factor = lu(zeros(0,0))
+
+    x = isempty(x0) ? fill(NaN,n) : copy(x0)
+    y,H2xpf = zeros(n),zeros(n)
+
+    kkt_buffer = Float64[]
+    sizehint!(kkt_buffer,4*(n+1)*(n+1))
+
+    daqp_workspace = isnothing(daqp_workspace) ? Model() : daqp_workspace;
+    DAQPBase.settings(daqp_workspace, Dict(:iter_limit => settings.inner_iter_limit, 
+                                           :primal_tol => settings.primal_tol, 
+                                           :dual_tol => settings.dual_tol))
+    exitflag,tsetup = DAQPBase.setup(daqp_workspace, H1pI, H2xpf, A, bu, bl, sense)
+
+    if m > size(A,1)
+        A = [I(n)[1:m-size(A,1),:];A] # To handle simple bounds
+    end
+
+    return exitflag, AVIWorkspace(H,f,A,bu,bl,
+                                  H_factor,zeros(n),
+                                  H1pI,H2mI,H2pI,H2,H2xpf,
+                                  x,y,daqp_workspace,kkt_buffer,
+                                  settings)
+end
+
+function solve(ws::AVIWorkspace)
+    exitflag = 0
+    λstar, ASstar = zeros(0), Int[]
+    n = length(ws.f)
+    ϵp, ϵd = ws.settings.primal_tol, ws.settings.dual_tol
+    α,β = ws.settings.alpha, ws.settings.beta
+    tot_iter,outer_iter,nkkt = 0,0,0
+    isnan(ws.x[1]) && (ws.x .= -ws.H\ws.f) # Use unconstrained optimum as x0
+    @inbounds for k in 1:ws.settings.iter_limit
+        ws.H2xpf .= ws.f
+        mul!(ws.H2xpf, ws.H2mI, ws.x, 1.0, 1.0)
+        DAQPBase.update(ws.daqp_workspace, nothing, ws.H2xpf, nothing, nothing,nothing)
+        ws.y[:], _, exitflag, info = DAQPBase.solve(ws.daqp_workspace)
+        exitflag < 0 && break 
+        tot_iter += info.iterations
+
+        # Same AS -> Check if KKT conditions are satisfied
+        if info.iterations == 1
+            nkkt += 1
+            ASu,ASl = _get_AS(info.λ,ϵd) 
+            #xt,λ,AS = _solve_kkt_schur(ws,ASu,ASl)
+            xt,λ,AS = _solve_kkt(ws,ASu,ASl)
+            nu = length(ASu)
+            if all(λ[i] > -ϵd for i in 1:nu) && all(λ[i] < ϵd for i in nu+1:length(λ))
+                Ax = ws.A*xt
+                if all(Ax-ws.bu .<= ϵp) && all(Ax-ws.bl .>=-ϵp)
+                    ws.x .= xt
+                    λstar,ASstar = copy(λ),AS
+                    exitflag, outer_iter = 1,k
+                    break
+                end
+            end
+            axpby!(β,xt,1.0-β,ws.x)
+        else
+            axpby!(1.0-α,ws.x,α,ws.y)
+            mul!(ws.y, ws.H2, ws.x, 1.0, 1.0)
+            ldiv!(ws.x, ws.H2pI, ws.y)
+        end
+        k == ws.settings.iter_limit && (exitflag = -4)
+    end
+    info = (status=flag2status[exitflag], AS=ASstar, outer_iterations=outer_iter, iterations=tot_iter, nkkt=nkkt)
+    return ws.x,λstar,exitflag,info
+end
+
 function _get_AS(λ,dual_tol)
     ASu,ASl = Int[],Int[]
     for i in 1:length(λ)
@@ -10,17 +127,57 @@ function _get_AS(λ,dual_tol)
     return ASu,ASl
 end
 
-function _solve_kkt(H,f,A,bu,bl,ASu,ASl,kkt_buffer)
+# TODO kkt_schur is faster in isolation, but leads to slower total execution 
+#function _solve_kkt_schur(ws::AVIWorkspace,ASu,ASl)
+#    # Prepare some helpers
+#    AS = [ASu;ASl]
+#    n,nAS = length(ws.f),length(AS)
+#
+#    # Juggle some buffers
+#    resize!(ws.kkt_buffer,nAS*(nAS+n+1)+n) # Is this necessary?
+#    invH_At = reshape(view(ws.kkt_buffer,1:nAS*n),(n,nAS))
+#    offset = nAS*n 
+#    S = reshape(view(ws.kkt_buffer,offset+1:offset+nAS^2),(nAS,nAS))
+#    offset +=nAS^2
+#    λ = view(ws.kkt_buffer,offset+1:offset+nAS)
+#    offset +=nAS
+#    x = view(ws.kkt_buffer,offset+1:offset+n)
+#
+#    AAS = ws.A[AS,:]
+#
+#    # Schur complement 
+#    @inbounds for (i,id) in enumerate(AS)
+#        @views invH_At[:,i] .= ws.A[id,:]
+#    end
+#    ldiv!(ws.H_factor,invH_At)
+#    mul!(S, AAS, invH_At)
+#    S_fact = lu!(S) 
+#
+#    # Solve for λ
+#    @views λ .= AAS * ws.x_unc
+#    @views λ[1:length(ASu)] .-= ws.bu[ASu]
+#    @views λ[length(ASu)+1:end] .-= ws.bl[ASl]
+#    ldiv!(S_fact,λ)
+#
+#    # Solve for x
+#    x .= ws.x_unc .- invH_At * λ
+#
+#    return x,λ,AS 
+#end
+function _solve_kkt(ws,ASu,ASl)
     AS = [ASu;ASl]
-    n = length(f)
+    n = length(ws.f)
     nkkt = n+length(AS)
-    resize!(kkt_buffer,nkkt^2)
-    K = reshape(view(kkt_buffer,1:nkkt^2),(nkkt,nkkt))
-    @views K[1:n,1:n] = H
-    @views K[n+1:end,1:n] = A[AS,:] 
-    @views K[1:n,n+1:end] = A[AS,:]'
+    resize!(ws.kkt_buffer,nkkt^2)
+    K = reshape(view(ws.kkt_buffer,1:nkkt^2),(nkkt,nkkt))
+    @views K[1:n,1:n] = ws.H
+    @views K[n+1:end,1:n] = ws.A[AS,:] 
+    @views K[1:n,n+1:end] = ws.A[AS,:]'
     K[n+1:end,n+1:end] .= 0
-    return K\[-f;bu[ASu];bl[ASl]], AS
+    z = K\[-ws.f;ws.bu[ASu];ws.bl[ASl]]
+    x = @view z[1:n]
+    λ = @view z[n+1:end]
+    return x,λ,AS
 end
 
 """
@@ -47,58 +204,7 @@ x ∈ {x: blower ≤ Ax ≤ bupper} such that:
 * `info` 		- tuple containing extra information from the solver such as status, active set and number of iterations.
 
 """
-function solve_avi(H::AbstractMatrix,f::AbstractVector,A::AbstractMatrix,bupper::AbstractVector,blower::AbstractVector=Float64[];
-        alpha=1.0,beta=0.25,primal_tol=1e-6, dual_tol = 1e-12, x0 = zeros(0), iter_limit=1000, inner_iter_limit=10000)
-
-    # Solves VI(Hx +h, C) where C={x| blower <= Ax <= bupper} using Douglas-Rachford
-    n,m = length(f),length(bupper)
-    blower = isempty(blower) ? fill(-1e30,m) : blower
-
-    H1 = (H+H') / 4
-    H2 = H1+(H-H') / 2
-    H1pI,H2mI,H2pI = H1+I, H2-I, lu!(H2+I)
-
-    x = isempty(x0) ? zeros(n) : copy(x0)
-    y,H2xpf = zeros(n),zeros(n),zeros(n)
-
-    backstep = DAQPBase.Model()
-    DAQPBase.settings(backstep, Dict(:iter_limit =>inner_iter_limit, :primal_tol=>primal_tol, :dual_tol=>dual_tol))
-    exitflag,tsetup = DAQPBase.setup(backstep, H1pI, H2xpf, A, bupper, blower)
-    exitflag < 0 && return nothing,nothing, (status=:Infeasible, exitflag=exitflag, outer_iterations=0, iterations=0) 
-
-    tot_iter = 0
-
-    kkt_buffer = Float64[]
-    sizehint!(kkt_buffer,4*(n+1)*(n+1))
-
-    @inbounds for k in 1:iter_limit
-        H2xpf .= f
-        mul!(H2xpf, H2mI, x, 1.0, 1.0)
-        DAQPBase.update(backstep, nothing, H2xpf, nothing, nothing,nothing)
-
-        y[:], _, exitflag, info = DAQPBase.solve(backstep)
-        exitflag < 0 && return nothing,nothing, (status=:Infeasible, exitflag=exitflag, outer_iterations=k, iterations=tot_iter) 
-        tot_iter += info.iterations
-
-        # Same AS -> Check if KKT conditions are satisfied
-        if info.iterations == 1
-            ASu,ASl = _get_AS(info.λ,dual_tol) 
-            z,AS = _solve_kkt(H,f,A,bupper,blower,ASu,ASl,kkt_buffer)
-            xt = @view z[1:n]
-            λ = @view z[n+1:end]
-            if all(λ[i] > -dual_tol for i in 1:length(ASu)) && all(λ[i] < dual_tol for i in length(ASu)+1:length(λ))
-                Ax = A*xt
-                if all(Ax-bupper .<= primal_tol) && all(Ax-blower .>=-primal_tol)
-                    return copy(xt),copy(λ),(AS=AS, status=:Solved, outer_iterations=k,iterations=tot_iter)
-                end
-            end
-            axpby!(beta,xt,1.0-beta,x)
-        else
-            axpby!(1.0-alpha,x,alpha,y)
-            mul!(y, H2, x, 1.0, 1.0)
-            ldiv!(x, H2pI, y)
-        end
-    end
-    return x,nothing, (status=:MaximumIterations, outer_iterations=iter_limit, iterations=tot_iter)
+function solve_avi(H::AbstractMatrix,f::AbstractVector,A::AbstractMatrix,bupper::AbstractVector,blower::AbstractVector=Float64[], sense=Cint[]; x0 = zeros(0), settings=AVISettings())
+    exitflag, ws = setup_avi(H,f,A,bupper,blower,sense;x0,settings)
+    return solve(ws)
 end
-