Make Hessian sparsity detection work with SCT (prototype)

Project.toml

@@ -16,6 +16,7 @@ PrecompileTools = "aea7be01-6a6a-4083-8856-8a6e6704d82a"
 ProgressLogging = "33c8b6b6-d38a-422a-b730-caa89a2f386c"
 Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 RecipesBase = "3cdcf5f2-1ef4-517c-9805-6587b60abb01"
+SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"
 SparseConnectivityTracer = "9f842d2f-2579-4b1d-911e-f412cf18a3f5"
 SparseMatrixColorings = "0a514795-09f3-496d-8182-132a7b665d35"
 
@@ -36,6 +37,7 @@ PrecompileTools = "1"
 ProgressLogging = "0.1"
 Random = "1.10"
 RecipesBase = "1"
+SparseArrays = "1.11.0"
 SparseConnectivityTracer = "0.6.13"
 SparseMatrixColorings = "0.4.14"
 TestItemRunner = "1"

src/ModelPredictiveControl.jl

@@ -3,6 +3,7 @@ module ModelPredictiveControl
 using PrecompileTools 
 using LinearAlgebra
 using Random: randn
+using SparseArrays
 
 using RecipesBase
 using ProgressLogging
@@ -49,6 +50,7 @@ include("state_estim.jl")
 include("predictive_control.jl")
 include("plot_sim.jl")
 
+#=
 @setup_workload begin
     # Putting some things in `@setup_workload` instead of `@compile_workload` can reduce the
     # size of the precompile file and potentially make loading faster.
@@ -58,5 +60,6 @@ include("plot_sim.jl")
         include("precompile.jl")
     end
 end
+=#
 
 end

src/controller/construct.jl

@@ -1,11 +1,11 @@
-struct PredictiveControllerBuffer{NT<:Real}
+struct PredictiveControllerBuffer{NT<:Real,M<:AbstractMatrix{NT}}
     u ::Vector{NT}
     Z̃ ::Vector{NT}
     D̂ ::Vector{NT}
     Ŷ ::Vector{NT}
     U ::Vector{NT}
     Ẽ ::Matrix{NT}
-    P̃u::Matrix{NT}
+    P̃u::M
     empty::Vector{NT}
 end
 
@@ -29,14 +29,19 @@ function PredictiveControllerBuffer(
     Ẽ  = Matrix{NT}(undef, ny*Hp, nZ̃)
     P̃u = Matrix{NT}(undef, nu*Hp, nZ̃)
     empty = Vector{NT}(undef, 0)
-    return PredictiveControllerBuffer{NT}(u, Z̃, D̂, Ŷ, U, Ẽ, P̃u, empty)
+    return PredictiveControllerBuffer{NT,typeof(P̃u)}(u, Z̃, D̂, Ŷ, U, Ẽ, P̃u, empty)
 end
 
 "Include all the objective function weights of [`PredictiveController`](@ref)"
-struct ControllerWeights{NT<:Real}
-    M_Hp::Hermitian{NT, Matrix{NT}}
-    Ñ_Hc::Hermitian{NT, Matrix{NT}}
-    L_Hp::Hermitian{NT, Matrix{NT}}
+struct ControllerWeights{
+    NT<:Real,
+    H1<:Hermitian{NT, <:AbstractMatrix{NT}},
+    H2<:Hermitian{NT, <:AbstractMatrix{NT}},
+    H3<:Hermitian{NT, <:AbstractMatrix{NT}},
+}
+    M_Hp::H1
+    Ñ_Hc::H2
+    L_Hp::H3
     E   ::NT
     iszero_M_Hp::Vector{Bool}
     iszero_Ñ_Hc::Vector{Bool}
@@ -46,15 +51,15 @@ struct ControllerWeights{NT<:Real}
         model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0
     ) where NT<:Real
         validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
-        # convert `Diagonal` to normal `Matrix` if required:
-        M_Hp = Hermitian(convert(Matrix{NT}, M_Hp), :L) 
-        N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L)
-        L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L)
+        M_Hp = Hermitian(convert.(NT, M_Hp), :L) 
+        N_Hc = Hermitian(convert.(NT, N_Hc), :L)
+        L_Hp = Hermitian(convert.(NT, L_Hp), :L)
         nΔU = size(N_Hc, 1)
         C = Cwt
         if !isinf(Cwt)  
             # ΔŨ = [ΔU; ϵ] (ϵ is the slack variable)
-            Ñ_Hc = Hermitian([N_Hc zeros(NT, nΔU, 1); zeros(NT, 1, nΔU) C], :L)
+            # Ñ_Hc = Hermitian([N_Hc zeros(NT, nΔU, 1); zeros(NT, 1, nΔU) C], :L)
+            Ñ_Hc = Hermitian(blockdiag(sparse(N_Hc), sparse(Diagonal([C]))), :L)
         else
             # ΔŨ = ΔU (only hard constraints)
             Ñ_Hc = N_Hc
@@ -64,7 +69,7 @@ struct ControllerWeights{NT<:Real}
         iszero_Ñ_Hc = [iszero(Ñ_Hc)]
         iszero_L_Hp = [iszero(L_Hp)]
         iszero_E = iszero(E)
-        return new{NT}(M_Hp, Ñ_Hc, L_Hp, E, iszero_M_Hp, iszero_Ñ_Hc, iszero_L_Hp, iszero_E)
+        return new{NT,typeof(M_Hp),typeof(Ñ_Hc),typeof(L_Hp)}(M_Hp, Ñ_Hc, L_Hp, E, iszero_M_Hp, iszero_Ñ_Hc, iszero_L_Hp, iszero_E)
     end
 end
 
@@ -584,10 +589,10 @@ function relaxU(Pu::Matrix{NT}, C_umin, C_umax, nϵ) where NT<:Real
         # ϵ impacts Z → U conversion for constraint calculations:
         A_Umin, A_Umax = -[Pu  C_umin], [Pu -C_umax] 
         # ϵ has no impact on Z → U conversion for prediction calculations:
-        P̃u = [Pu zeros(NT, size(Pu, 1))]
+        P̃u = sparse_hcat(sparse(Pu), spzeros(NT, size(Pu, 1)))
     else # Z̃ = Z (only hard constraints)
         A_Umin, A_Umax = -Pu,  Pu
-        P̃u = Pu
+        P̃u = sparse(Pu)
     end
     return A_Umin, A_Umax, P̃u
 end
@@ -621,17 +626,17 @@ bound, which is more precise than a linear inequality constraint. However, it is
 convenient to treat it as a linear inequality constraint since the optimizer `OSQP.jl` does
 not support pure bounds on the decision variables.
 """
-function relaxΔU(PΔu::Matrix{NT}, C_Δumin, C_Δumax, ΔUmin, ΔUmax, nϵ) where NT<:Real
+function relaxΔU(PΔu::AbstractMatrix{NT}, C_Δumin, C_Δumax, ΔUmin, ΔUmax, nϵ) where NT<:Real
     nZ = size(PΔu, 2)
     if nϵ == 1 # Z̃ = [Z; ϵ]
         ΔŨmin, ΔŨmax = [ΔUmin; NT[0.0]], [ΔUmax; NT[Inf]] # 0 ≤ ϵ ≤ ∞
         A_ϵ = [zeros(NT, 1, nZ) NT[1.0]]
         A_ΔŨmin, A_ΔŨmax = -[PΔu  C_Δumin; A_ϵ], [PΔu -C_Δumax; A_ϵ]
-        P̃Δu = [PΔu zeros(NT, size(PΔu, 1), 1); zeros(NT, 1, size(PΔu, 2)) NT[1.0]]
+        P̃Δu = blockdiag(sparse(PΔu), spdiagm([one(NT)]))
     else # Z̃ = Z (only hard constraints)
         ΔŨmin, ΔŨmax = ΔUmin, ΔUmax
         A_ΔŨmin, A_ΔŨmax = -PΔu,  PΔu
-        P̃Δu = PΔu
+        P̃Δu = sparse(PΔu)
     end
     return A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃Δu
 end

src/controller/execute.jl

@@ -370,7 +370,12 @@ function obj_nonlinprog!(
     end
     # --- economic term ---
     E_JE = obj_econ(mpc, model, Ue, Ŷe)
-    return JR̂y + JΔŨ + JR̂u + E_JE
+    return (
+        JR̂y +
+        JΔŨ +
+        JR̂u +
+        E_JE
+    )
 end
 
 "No custom nonlinear constraints `gc` by default, return `gc` unchanged." 

src/controller/nonlinmpc.jl

@@ -18,7 +18,8 @@ struct NonLinMPC{
     JB<:AbstractADType,
     PT<:Any,
     JEfunc<:Function,
-    GCfunc<:Function
+    GCfunc<:Function,
+    M<:AbstractMatrix{NT}
 } <: PredictiveController{NT}
     estim::SE
     transcription::TM
@@ -39,8 +40,8 @@ struct NonLinMPC{
     p::PT
     R̂u::Vector{NT}
     R̂y::Vector{NT}
-    P̃Δu::Matrix{NT}
-    P̃u ::Matrix{NT}
+    P̃Δu::M
+    P̃u ::M
     Tu ::Matrix{NT}
     Tu_lastu0::Vector{NT}
     Ẽ::Matrix{NT}
@@ -110,7 +111,7 @@ struct NonLinMPC{
         nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ
         Z̃ = zeros(NT, nZ̃)
         buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ)
-        mpc = new{NT, SE, TM, JM, GB, HB, JB, PT, JEfunc, GCfunc}(
+        mpc = new{NT, SE, TM, JM, GB, HB, JB, PT, JEfunc, GCfunc, typeof(P̃u)}(
             estim, transcription, optim, con,
             gradient, hessian, jacobian,
             Z̃, ŷ,
@@ -289,9 +290,9 @@ function NonLinMPC(
     Mwt  = fill(DEFAULT_MWT, model.ny),
     Nwt  = fill(DEFAULT_NWT, model.nu),
     Lwt  = fill(DEFAULT_LWT, model.nu),
-    M_Hp = diagm(repeat(Mwt, Hp)),
-    N_Hc = diagm(repeat(Nwt, Hc)),
-    L_Hp = diagm(repeat(Lwt, Hp)),
+    M_Hp = Diagonal(repeat(Mwt, Hp)),
+    N_Hc = Diagonal(repeat(Nwt, Hc)),
+    L_Hp = Diagonal(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
     JE ::Function = (_,_,_,_) -> 0.0,
@@ -321,9 +322,9 @@ function NonLinMPC(
     Mwt  = fill(DEFAULT_MWT, model.ny),
     Nwt  = fill(DEFAULT_NWT, model.nu),
     Lwt  = fill(DEFAULT_LWT, model.nu),
-    M_Hp = diagm(repeat(Mwt, Hp)),
-    N_Hc = diagm(repeat(Nwt, Hc)),
-    L_Hp = diagm(repeat(Lwt, Hp)),
+    M_Hp = Diagonal(repeat(Mwt, Hp)),
+    N_Hc = Diagonal(repeat(Nwt, Hc)),
+    L_Hp = Diagonal(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
     JE ::Function = (_,_,_,_) -> 0.0,
@@ -377,9 +378,9 @@ function NonLinMPC(
     Mwt  = fill(DEFAULT_MWT, estim.model.ny),
     Nwt  = fill(DEFAULT_NWT, estim.model.nu),
     Lwt  = fill(DEFAULT_LWT, estim.model.nu),
-    M_Hp = diagm(repeat(Mwt, Hp)),
-    N_Hc = diagm(repeat(Nwt, Hc)),
-    L_Hp = diagm(repeat(Lwt, Hp)),
+    M_Hp = Diagonal(repeat(Mwt, Hp)),
+    N_Hc = Diagonal(repeat(Nwt, Hc)),
+    L_Hp = Diagonal(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
     JE ::Function = (_,_,_,_) -> 0.0,

src/controller/transcription.jl

@@ -85,15 +85,15 @@ function init_ZtoΔU end
 function init_ZtoΔU(
     estim::StateEstimator{NT}, transcription::SingleShooting, _ , Hc
 ) where {NT<:Real}
-    PΔu = Matrix{NT}(I, estim.model.nu*Hc, estim.model.nu*Hc)
+    PΔu = Diagonal(fill(one(NT), estim.model.nu*Hc))
     return PΔu
 end
 
 function init_ZtoΔU(
     estim::StateEstimator{NT}, transcription::MultipleShooting, Hp, Hc
 ) where {NT<:Real}
-    I_nu_Hc = Matrix{NT}(I, estim.model.nu*Hc, estim.model.nu*Hc)
-    PΔu = [I_nu_Hc zeros(NT, estim.model.nu*Hc, estim.nx̂*Hp)]
+    I_nu_Hc = Diagonal(fill(one(NT), estim.model.nu*Hc))
+    PΔu = sparse_hcat(I_nu_Hc , spzeros(NT, estim.model.nu*Hc, estim.nx̂*Hp))
     return PΔu
 end
 
@@ -145,7 +145,8 @@ function init_ZtoU(
 ) where {NT<:Real}
     model = estim.model
     # Pu and Tu are `Matrix{NT}`, conversion is faster than `Matrix{Bool}` or `BitMatrix`
-    I_nu = Matrix{NT}(I, model.nu, model.nu)
+    I_nu = Diagonal(fill(one(NT), model.nu))
+    # TODO: make PU and friends sparse
     PU_Hc = LowerTriangular(repeat(I_nu, Hc, Hc))
     PUdagger = [PU_Hc; repeat(I_nu, Hp - Hc, Hc)]
     Pu = init_PUmat(estim, transcription, Hp, Hc, PUdagger)