diff --git a/interfaces/daqp-julia/Project.toml b/interfaces/daqp-julia/Project.toml index fc79260..80a1148 100644 --- a/interfaces/daqp-julia/Project.toml +++ b/interfaces/daqp-julia/Project.toml @@ -1,7 +1,7 @@ name = "DAQPBase" uuid = "833476c3-a8c0-4073-9b64-6473509843fe" authors = ["Daniel Arnström "] -version = "0.3.4" +version = "0.3.5" [deps] DAQP_jll = "5c51c916-43bf-57fe-9d62-13064ebbf40d" diff --git a/interfaces/daqp-julia/src/avi.jl b/interfaces/daqp-julia/src/avi.jl index 0fd1165..2a19ca9 100644 --- a/interfaces/daqp-julia/src/avi.jl +++ b/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 -