diff --git a/Project.toml b/Project.toml index 3c72934f..73b90731 100644 --- a/Project.toml +++ b/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" diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index 70c0e360..b4a7de3d 100644 --- a/src/ModelPredictiveControl.jl +++ b/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 \ No newline at end of file diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 16b7b745..9f53adf8 100644 --- a/src/controller/construct.jl +++ b/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 diff --git a/src/controller/execute.jl b/src/controller/execute.jl index a72f7d74..2649ef86 100644 --- a/src/controller/execute.jl +++ b/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." diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 41859e71..267e54ff 100644 --- a/src/controller/nonlinmpc.jl +++ b/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, diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index b737092f..e02c3c69 100644 --- a/src/controller/transcription.jl +++ b/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)