From 5c2b0cfc570a050e5c686521894b03dfc81e3ef8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 28 Nov 2024 17:58:19 -0500 Subject: [PATCH 01/10] wip: all field in struct should be concrete type `ControllerConstraint{NT}` is no longer concrete `ControllerConstraint{NT, GCfunc}` is --- src/controller/execute.jl | 7 +++---- src/controller/nonlinmpc.jl | 32 +++++++++++++++++++++----------- 2 files changed, 24 insertions(+), 15 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 040853da..68c412fd 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -371,11 +371,10 @@ The function mutates `Ue`, `Ŷe` and `Ū` in arguments, without assuming any i function extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ) ny, nu = model.ny, model.nu # --- extended manipulated inputs Ue = [U; u(k+Hp-1)] --- - U0 = Ū - U0 .= mul!(U0, mpc.S̃, ΔŨ) .+ mpc.T_lastu0 - Ue[1:end-nu] .= U0 .+ mpc.Uop + U = Ū + U .= mul!(U, mpc.S̃, ΔŨ) .+ mpc.T_lastu0 .+ mpc.Uop # u(k + Hp) = u(k + Hp - 1) since Δu(k+Hp) = 0 (because Hc ≤ Hp): - Ue[end-nu+1:end] .= @views Ue[end-2nu+1:end-nu] + Ue[end-nu+1:end] .= @views U[end-nu+1:end] # --- extended output predictions Ŷe = [ŷ(k); Ŷ] --- Ŷe[1:ny] .= mpc.ŷ Ŷe[ny+1:end] .= Ŷ0 .+ mpc.Yop diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 29671e88..1bb4f406 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -2,16 +2,17 @@ const DEFAULT_NONLINMPC_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"s struct NonLinMPC{ NT<:Real, - SE<:StateEstimator, + SE<:StateEstimator, JM<:JuMP.GenericModel, JEfunc<:Function, + GCfunc<:Function, P<:Any } <: PredictiveController{NT} estim::SE # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be # different since solvers that support non-Float64 are scarce. optim::JM - con::ControllerConstraint{NT} + con::ControllerConstraint{NT, GCfunc} ΔŨ::Vector{NT} ŷ ::Vector{NT} Hp::Int @@ -48,9 +49,17 @@ struct NonLinMPC{ Yop::Vector{NT} Dop::Vector{NT} buffer::PredictiveControllerBuffer{NT} - function NonLinMPC{NT, SE, JM, JEfunc, P}( - estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gc, nc, p::P, optim::JM - ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel, JEfunc<:Function, P<:Any} + function NonLinMPC{NT, SE, JM, JEfunc, GCfunc, P}( + estim::SE, + Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gc::GCfunc, nc, p::P, optim::JM + ) where { + NT<:Real, + SE<:StateEstimator, + JM<:JuMP.GenericModel, + JEfunc<:Function, + GCfunc<:Function, + P<:Any + } model = estim.model nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂ ŷ = copy(model.yop) # dummy vals (updated just before optimization) @@ -82,7 +91,7 @@ struct NonLinMPC{ nΔŨ = size(Ẽ, 2) ΔŨ = zeros(NT, nΔŨ) buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp) - mpc = new{NT, SE, JM, JEfunc, P}( + mpc = new{NT, SE, JM, JEfunc, GCfunc, P}( estim, optim, con, ΔŨ, ŷ, Hp, Hc, nϵ, @@ -318,9 +327,9 @@ function NonLinMPC( L_Hp = diagm(repeat(Lwt, Hp)), Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, - JE ::JEfunc = (_,_,_,_) -> 0.0, + JE ::JEfunc = (_,_,_,_) -> 0.0, gc!::Function = (_,_,_,_,_,_) -> nothing, - gc ::Function = gc!, + gc ::GCfunc = gc!, nc = 0, p::P = estim.model.p, optim::JM = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), @@ -329,6 +338,7 @@ function NonLinMPC( SE<:StateEstimator{NT}, JM<:JuMP.GenericModel, JEfunc<:Function, + GCfunc<:Function, P<:Any } nk = estimate_delays(estim.model) @@ -336,7 +346,7 @@ function NonLinMPC( @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "* "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)") end - return NonLinMPC{NT, SE, JM, JEfunc, P}( + return NonLinMPC{NT, SE, JM, JEfunc, GCfunc, P}( estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, gc, nc, p, optim ) end @@ -520,7 +530,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT gc = get_tmp(gc_cache, ΔŨ1) Ŷ0, x̂0end = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ) Ue, Ŷe = extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ) - ϵ = (nϵ ≠ 0) ? ΔŨ[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation) + ϵ = (nϵ ≠ 0) ? ΔŨ[end] : 0 # ϵ = 0 if nϵ == 0 (meaning no relaxation) mpc.con.gc!(gc, Ue, Ŷe, mpc.D̂e, mpc.p, ϵ) g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ) return obj_nonlinprog!(Ȳ, Ū, mpc, model, Ue, Ŷe, ΔŨ)::T @@ -539,7 +549,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT gc = get_tmp(gc_cache, ΔŨ1) Ŷ0, x̂0end = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ) Ue, Ŷe = extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ) - ϵ = (nϵ ≠ 0) ? ΔŨ[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation) + ϵ = (nϵ ≠ 0) ? ΔŨ[end] : 0 # ϵ = 0 if nϵ == 0 (meaning no relaxation) mpc.con.gc!(gc, Ue, Ŷe, mpc.D̂e, mpc.p, ϵ) g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ) end From c69e7f8c2e1729eebc8c5df9ed998c47c835366a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Dec 2024 10:36:11 -0500 Subject: [PATCH 02/10] debug: extended input `Ue` calculated correctly --- src/controller/execute.jl | 3 ++- src/controller/nonlinmpc.jl | 17 ++++++++++------- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index b226d18e..a5980a53 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -373,12 +373,13 @@ function extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ) # --- extended manipulated inputs Ue = [U; u(k+Hp-1)] --- U = Ū U .= mul!(U, mpc.S̃, ΔŨ) .+ mpc.T_lastu0 .+ mpc.Uop + Ue[1:end-nu] .= U # u(k + Hp) = u(k + Hp - 1) since Δu(k+Hp) = 0 (because Hc ≤ Hp): Ue[end-nu+1:end] .= @views U[end-nu+1:end] # --- extended output predictions Ŷe = [ŷ(k); Ŷ] --- Ŷe[1:ny] .= mpc.ŷ Ŷe[ny+1:end] .= Ŷ0 .+ mpc.Yop - return Ue, Ŷe + return Ue, Ŷe end """ diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index e8b0f54e..fe64686e 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -48,7 +48,7 @@ struct NonLinMPC{ buffer::PredictiveControllerBuffer{NT} function NonLinMPC{NT, SE, JM, JEfunc, GCfunc, P}( estim::SE, - Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gc::GCfunc, nc, p::P, optim::JM + Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gc!::GCfunc, nc, p::P, optim::JM ) where { NT<:Real, SE<:StateEstimator, @@ -60,8 +60,6 @@ struct NonLinMPC{ model = estim.model nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂ ŷ = copy(model.yop) # dummy vals (updated just before optimization) - validate_JE(NT, JE) - gc! = get_mutating_gc(NT, gc) weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) # dummy vals (updated just before optimization): R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) @@ -243,7 +241,7 @@ function NonLinMPC( L_Hp = diagm(repeat(Lwt, Hp)), Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, - JE::Function = (_,_,_,_) -> 0.0, + JE ::Function = (_,_,_,_) -> 0.0, gc!::Function = (_,_,_,_,_,_) -> nothing, gc ::Function = gc!, nc::Int = 0, @@ -323,7 +321,7 @@ function NonLinMPC( Ewt = DEFAULT_EWT, JE ::JEfunc = (_,_,_,_) -> 0.0, gc!::Function = (_,_,_,_,_,_) -> nothing, - gc ::GCfunc = gc!, + gc ::Function = gc!, nc = 0, p::P = estim.model.p, optim::JM = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), @@ -332,7 +330,6 @@ function NonLinMPC( SE<:StateEstimator{NT}, JM<:JuMP.GenericModel, JEfunc<:Function, - GCfunc<:Function, P<:Any } nk = estimate_delays(estim.model) @@ -340,8 +337,11 @@ function NonLinMPC( @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "* "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)") end + validate_JE(NT, JE) + gc! = get_mutating_gc(NT, gc) + GCfunc = get_type_mutating_gc(gc!) return NonLinMPC{NT, SE, JM, JEfunc, GCfunc, P}( - estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, gc, nc, p, optim + estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, gc!, nc, p, optim ) end @@ -398,6 +398,9 @@ function get_mutating_gc(NT, gc) return gc! end +"Get the type of the mutating version of the custom constrain function `gc!`." +get_type_mutating_gc(::GCfunc) where {GCfunc<:Function} = GCfunc + """ test_custom_functions(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p) From 9f663d1056055860f98c34fb7f3d61102e98a254 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Dec 2024 10:50:48 -0500 Subject: [PATCH 03/10] test: new custom constraint violation tests that use `Ue` arg --- test/test_predictive_control.jl | 24 +++++++++++++++++++----- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/test/test_predictive_control.jl b/test/test_predictive_control.jl index 5afbbfe0..0346470b 100644 --- a/test/test_predictive_control.jl +++ b/test/test_predictive_control.jl @@ -714,10 +714,10 @@ end end @testset "NonLinMPC constraint violation" begin - gc( _ , Ŷe, _ , p , ϵ) = p[]*(Ŷe .- 3.14 .- ϵ) + gc(Ue, Ŷe, _ ,p , ϵ) = [p[1]*(Ue .- 4.2 .- ϵ); p[2]*(Ŷe .- 3.14 .- ϵ)] linmodel = LinModel(tf([2], [10000, 1]), 3000.0) - nmpc_lin = NonLinMPC(linmodel, Hp=50, Hc=5, gc=gc, nc=50+1, p=[0]) + nmpc_lin = NonLinMPC(linmodel, Hp=50, Hc=5, gc=gc, nc=2*(50+1), p=[0; 0]) setconstraint!(nmpc_lin, x̂min=[-1e3,-Inf], x̂max=[1e3,+Inf]) setconstraint!(nmpc_lin, umin=[-3], umax=[3]) @@ -766,11 +766,18 @@ end info = getinfo(nmpc_lin) @test info[:x̂end][1] ≈ 0 atol=1e-1 - nmpc_lin.p[] = 1 setconstraint!(nmpc_lin, x̂min=[-1e3,-Inf], x̂max=[1e3,+Inf]) setconstraint!(nmpc_lin, umin=[-10], umax=[10]) setconstraint!(nmpc_lin, Δumin=[-15], Δumax=[15]) setconstraint!(nmpc_lin, ymin=[-100], ymax=[100]) + + nmpc_lin.p .= [1; 0] + moveinput!(nmpc_lin, [20]) + info = getinfo(nmpc_lin) + @test info[:U][end] ≈ 4.2 atol=1e-1 + @test info[:U][begin] ≈ 4.2 atol=1e-1 + + nmpc_lin.p .= [0; 1] moveinput!(nmpc_lin, [20]) info = getinfo(nmpc_lin) @test info[:Ŷ][end] ≈ 3.14 atol=1e-1 @@ -779,7 +786,7 @@ end f = (x,u,_,_) -> linmodel.A*x + linmodel.Bu*u h = (x,_,_) -> linmodel.C*x nonlinmodel = NonLinModel(f, h, linmodel.Ts, 1, 1, 1, solver=nothing) - nmpc = NonLinMPC(nonlinmodel, Hp=50, Hc=5, gc=gc, nc=50+1, p=[0]) + nmpc = NonLinMPC(nonlinmodel, Hp=50, Hc=5, gc=gc, nc=2*(50+1), p=[0; 0]) setconstraint!(nmpc, x̂min=[-1e3,-Inf], x̂max=[1e3,+Inf]) setconstraint!(nmpc, umin=[-3], umax=[3]) @@ -828,11 +835,18 @@ end info = getinfo(nmpc) @test info[:x̂end][1] ≈ 0 atol=1e-1 - nmpc.p[] = 1 setconstraint!(nmpc, x̂min=[-1e3,-Inf], x̂max=[1e3,+Inf]) setconstraint!(nmpc, umin=[-10], umax=[10]) setconstraint!(nmpc, Δumin=[-15], Δumax=[15]) setconstraint!(nmpc, ymin=[-100], ymax=[100]) + + nmpc.p .= [1; 0] + moveinput!(nmpc, [20]) + info = getinfo(nmpc) + @test info[:U][end] ≈ 4.2 atol=1e-1 + @test info[:U][begin] ≈ 4.2 atol=1e-1 + + nmpc.p .= [0; 1] moveinput!(nmpc, [20]) info = getinfo(nmpc) @test info[:Ŷ][end] ≈ 3.14 atol=1e-1 From 4ae6b07b03e80a85cb7a94861757c4ec8061ed9e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Dec 2024 11:24:53 -0500 Subject: [PATCH 04/10] fixed: `LinMPC` is now type-stable --- src/controller/construct.jl | 11 +++++------ src/controller/linmpc.jl | 2 +- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 89103e84..321042ed 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -27,7 +27,7 @@ struct ControllerWeights{NT<:Real} end "Include all the data for the constraints of [`PredictiveController`](@ref)" -struct ControllerConstraint{NT<:Real, GCfunc<:Function} +struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} ẽx̂ ::Matrix{NT} fx̂ ::Vector{NT} gx̂ ::Matrix{NT} @@ -668,7 +668,7 @@ end """ init_defaultcon_mpc( estim, C, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, - gc!=(_,_,_,_,_,_)->nothing, nc=0 + gc!=nothing, nc=0 ) -> con, S̃, Ẽ Init `ControllerConstraint` struct with default parameters based on estimator `estim`. @@ -676,10 +676,9 @@ Init `ControllerConstraint` struct with default parameters based on estimator `e Also return `S̃` and `Ẽ` matrices for the the augmented decision vector `ΔŨ`. """ function init_defaultcon_mpc( - estim::StateEstimator{NT}, - Hp, Hc, C, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, - gc!::GCfunc=(_,_,_,_,_,_)->nothing, nc=0 -) where {NT<:Real, GCfunc<:Function} + estim::StateEstimator{NT}, Hp, Hc, C, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, + gc!::GCfunc=nothing, nc=0 +) where {NT<:Real, GCfunc<:Union{Nothing, Function}} model = estim.model nu, ny, nx̂ = model.nu, model.ny, estim.nx̂ nϵ = isinf(C) ? 0 : 1 diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 66e42887..1e2a6eea 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -9,7 +9,7 @@ struct LinMPC{ # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be # different since solvers that support non-Float64 are scarce. optim::JM - con::ControllerConstraint{NT} + con::ControllerConstraint{NT, Nothing} ΔŨ::Vector{NT} ŷ ::Vector{NT} Hp::Int From 5b1b90c20133846c9c9b0775f4030aac5a6d4951 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Dec 2024 12:11:01 -0500 Subject: [PATCH 05/10] test: larger `Ts` in `NonLinMPC` set model to avoid time limits in CI --- test/test_predictive_control.jl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/test_predictive_control.jl b/test/test_predictive_control.jl index 0346470b..02f87627 100644 --- a/test/test_predictive_control.jl +++ b/test/test_predictive_control.jl @@ -854,7 +854,7 @@ end end @testset "NonLinMPC set model" begin - estim = KalmanFilter(setop!(LinModel(tf(5, [2, 1]), 3), yop=[10], uop=[1])) + estim = KalmanFilter(setop!(LinModel(tf(5, [200, 1]), 300), yop=[10], uop=[1])) mpc = NonLinMPC(estim, Nwt=[0], Cwt=1e4, Hp=1000, Hc=1) mpc = setconstraint!(mpc, umin=[-24], umax=[26]) mpc = setconstraint!(mpc, ymin=[-54], ymax=[56]) @@ -868,7 +868,7 @@ end preparestate!(mpc, [10]) u = moveinput!(mpc, r) @test u ≈ [2] atol=1e-2 - setmodel!(mpc, setop!(LinModel(tf(5, [2, 1]), 3), yop=[20], uop=[11])) + setmodel!(mpc, setop!(LinModel(tf(5, [200, 1]), 300), yop=[20], uop=[11])) @test mpc.Yop ≈ fill(20.0, 1000) @test mpc.Uop ≈ fill(11.0, 1000) @test mpc.con.U0min ≈ fill(-24.0 - 1 + 1 - 11, 1000) @@ -878,7 +878,7 @@ end r = [40] u = moveinput!(mpc, r) @test u ≈ [15] atol=1e-2 - setmodel!(mpc, setop!(LinModel(tf(10, [2, 1]), 3), yop=[20], uop=[11])) + setmodel!(mpc, setop!(LinModel(tf(10, [200, 1]), 300), yop=[20], uop=[11])) r = [40] u = moveinput!(mpc, r) @test u ≈ [13] atol=1e-2 From ecb274814ddc9cd13a8455c905006f7507e5205d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Dec 2024 12:16:45 -0500 Subject: [PATCH 06/10] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index e4364e03..3148df94 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "1.1.0" +version = "1.1.1" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" From 4dbd3541edfcae3d3496c8c7c3ce0a16ebf6fff6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Dec 2024 12:33:55 -0500 Subject: [PATCH 07/10] =?UTF-8?q?changed:=20removed=20`noR=CC=82u`=20field?= =?UTF-8?q?=20in=20all=20`PredictiveController`=20-=20it's=20dangerous=20w?= =?UTF-8?q?ith=20`setmodel!`=20(if=20`L=5FHp`=20change,=20`noR=CC=82u`=20c?= =?UTF-8?q?an=20be=20incorrect=20-=20the=20code=20is=20simpler=20(no=20bra?= =?UTF-8?q?nching)=20-=20computational=20gains=20are=20negligible?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/construct.jl | 4 +--- src/controller/execute.jl | 29 ++++++++++++----------------- src/controller/explicitmpc.jl | 4 +--- src/controller/linmpc.jl | 4 +--- src/controller/nonlinmpc.jl | 4 +--- 5 files changed, 16 insertions(+), 29 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 321042ed..e6c5e593 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -434,9 +434,7 @@ function validate_args(mpc::PredictiveController, ry, d, D̂, R̂y, R̂u) size(d) ≠ (nd,) && throw(DimensionMismatch("d size $(size(d)) ≠ measured dist. size ($nd,)")) size(D̂) ≠ (nd*Hp,) && throw(DimensionMismatch("D̂ size $(size(D̂)) ≠ measured dist. size × Hp ($(nd*Hp),)")) size(R̂y) ≠ (ny*Hp,) && throw(DimensionMismatch("R̂y size $(size(R̂y)) ≠ output size × Hp ($(ny*Hp),)")) - if ~mpc.noR̂u - size(R̂u) ≠ (nu*Hp,) && throw(DimensionMismatch("R̂u size $(size(R̂u)) ≠ manip. input size × Hp ($(nu*Hp),)")) - end + size(R̂u) ≠ (nu*Hp,) && throw(DimensionMismatch("R̂u size $(size(R̂u)) ≠ manip. input size × Hp ($(nu*Hp),)")) end diff --git a/src/controller/execute.jl b/src/controller/execute.jl index a5980a53..1bdde3a0 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -204,18 +204,19 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂ mul!(F, mpc.G, mpc.d0, 1, 1) mul!(F, mpc.J, mpc.D̂0, 1, 1) end + # --- output setpoint tracking term --- mpc.R̂y .= R̂y Cy = F .- (R̂y .- mpc.Yop) M_Hp_Ẽ = mpc.weights.M_Hp*mpc.Ẽ mul!(q̃, M_Hp_Ẽ', Cy) r .= dot(Cy, mpc.weights.M_Hp, Cy) - if ~mpc.noR̂u - mpc.R̂u .= R̂u - Cu = mpc.T_lastu0 .- (R̂u .- mpc.Uop) - L_Hp_S̃ = mpc.weights.L_Hp*mpc.S̃ - mul!(q̃, L_Hp_S̃', Cu, 1, 1) - r .+= dot(Cu, mpc.weights.L_Hp, Cu) - end + # --- input setpoint tracking term --- + mpc.R̂u .= R̂u + Cu = mpc.T_lastu0 .- (R̂u .- mpc.Uop) + L_Hp_S̃ = mpc.weights.L_Hp*mpc.S̃ + mul!(q̃, L_Hp_S̃', Cu, 1, 1) + r .+= dot(Cu, mpc.weights.L_Hp, Cu) + # --- finalize --- lmul!(2, q̃) return nothing end @@ -236,9 +237,7 @@ function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂ mpc.D̂e[model.nd+1:end] .= D̂ end mpc.R̂y .= R̂y - if ~mpc.noR̂u - mpc.R̂u .= R̂u - end + mpc.R̂u .= R̂u return nothing end @@ -418,13 +417,9 @@ function obj_nonlinprog!( # --- move suppression and slack variable term --- JΔŨ = dot(ΔŨ, mpc.weights.Ñ_Hc, ΔŨ) # --- input setpoint tracking term --- - if !mpc.noR̂u - Ū .= @views Ue[1:end-nu] - Ū .= mpc.R̂u .- Ū - JR̂u = dot(Ū, mpc.weights.L_Hp, Ū) - else - JR̂u = 0.0 - end + Ū .= @views Ue[1:end-nu] + Ū .= mpc.R̂u .- Ū + JR̂u = dot(Ū, mpc.weights.L_Hp, Ū) # --- economic term --- E_JE = obj_econ(mpc, model, Ue, Ŷe) return JR̂y + JΔŨ + JR̂u + E_JE diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 1d83d1db..979123b5 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -8,7 +8,6 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} weights::ControllerWeights{NT} R̂u::Vector{NT} R̂y::Vector{NT} - noR̂u::Bool S̃::Matrix{NT} T::Matrix{NT} T_lastu0::Vector{NT} @@ -46,7 +45,6 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L) # dummy vals (updated just before optimization): R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) - noR̂u = iszero(L_Hp) S, T = init_ΔUtoU(model, Hp, Hc) E, G, J, K, V, B = init_predmat(estim, model, Hp, Hc) # dummy val (updated just before optimization): @@ -68,7 +66,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} ΔŨ, ŷ, Hp, Hc, nϵ, weights, - R̂u, R̂y, noR̂u, + R̂u, R̂y, S̃, T, T_lastu0, Ẽ, F, G, J, K, V, B, H̃, q̃, r, diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 1e2a6eea..212e100a 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -18,7 +18,6 @@ struct LinMPC{ weights::ControllerWeights{NT} R̂u::Vector{NT} R̂y::Vector{NT} - noR̂u::Bool S̃::Matrix{NT} T::Matrix{NT} T_lastu0::Vector{NT} @@ -50,7 +49,6 @@ struct LinMPC{ weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt) # dummy vals (updated just before optimization): R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) - noR̂u = iszero(L_Hp) S, T = init_ΔUtoU(model, Hp, Hc) E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc) # dummy vals (updated just before optimization): @@ -73,7 +71,7 @@ struct LinMPC{ ΔŨ, ŷ, Hp, Hc, nϵ, weights, - R̂u, R̂y, noR̂u, + R̂u, R̂y, S̃, T, T_lastu0, Ẽ, F, G, J, K, V, B, H̃, q̃, r, diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index fe64686e..07556b0c 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -23,7 +23,6 @@ struct NonLinMPC{ p::P R̂u::Vector{NT} R̂y::Vector{NT} - noR̂u::Bool S̃::Matrix{NT} T::Matrix{NT} T_lastu0::Vector{NT} @@ -63,7 +62,6 @@ struct NonLinMPC{ weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) # dummy vals (updated just before optimization): R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) - noR̂u = iszero(L_Hp) S, T = init_ΔUtoU(model, Hp, Hc) E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc) # dummy vals (updated just before optimization): @@ -88,7 +86,7 @@ struct NonLinMPC{ Hp, Hc, nϵ, weights, JE, p, - R̂u, R̂y, noR̂u, + R̂u, R̂y, S̃, T, T_lastu0, Ẽ, F, G, J, K, V, B, H̃, q̃, r, From 11dc6b13b6f2265f18fc4fc2acf3e51f396a7542 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Dec 2024 13:13:23 -0500 Subject: [PATCH 08/10] added: reduce allocations `PredictiveController` with new buffer fields --- src/controller/execute.jl | 11 ++++++----- src/predictive_control.jl | 6 +++++- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 1bdde3a0..fd115588 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -191,6 +191,7 @@ They are computed with these equations using in-place operations: function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u) mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0) ŷ, F, q̃, r = mpc.ŷ, mpc.F, mpc.q̃, mpc.r + Cy, Cu = mpc.buffer.Cy, mpc.buffer.Cu ŷ .= evaloutput(mpc.estim, d) predictstoch!(mpc, mpc.estim) # init mpc.F with Ŷs for InternalModel F .+= mpc.B @@ -206,18 +207,18 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂ end # --- output setpoint tracking term --- mpc.R̂y .= R̂y - Cy = F .- (R̂y .- mpc.Yop) + Cy .= F .- (R̂y .- mpc.Yop) M_Hp_Ẽ = mpc.weights.M_Hp*mpc.Ẽ - mul!(q̃, M_Hp_Ẽ', Cy) + mul!(q̃, M_Hp_Ẽ', Cy) # q̃ = M_Hp*Ẽ'*Cy r .= dot(Cy, mpc.weights.M_Hp, Cy) # --- input setpoint tracking term --- mpc.R̂u .= R̂u - Cu = mpc.T_lastu0 .- (R̂u .- mpc.Uop) + Cu .= mpc.T_lastu0 .- (R̂u .- mpc.Uop) L_Hp_S̃ = mpc.weights.L_Hp*mpc.S̃ - mul!(q̃, L_Hp_S̃', Cu, 1, 1) + mul!(q̃, L_Hp_S̃', Cu, 1, 1) # q̃ = q̃ + L_Hp*S̃'*Cu r .+= dot(Cu, mpc.weights.L_Hp, Cu) # --- finalize --- - lmul!(2, q̃) + lmul!(2, q̃) # q̃ = 2*q̃ return nothing end diff --git a/src/predictive_control.jl b/src/predictive_control.jl index 54bc7186..1c0afa13 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -23,6 +23,8 @@ struct PredictiveControllerBuffer{NT<:Real} u ::Vector{NT} R̂y::Vector{NT} D̂ ::Vector{NT} + Cy::Vector{NT} + Cu::Vector{NT} empty::Vector{NT} end @@ -37,8 +39,10 @@ function PredictiveControllerBuffer{NT}(nu::Int, ny::Int, nd::Int, Hp::Int) wher u = Vector{NT}(undef, nu) R̂y = Vector{NT}(undef, ny*Hp) D̂ = Vector{NT}(undef, nd*Hp) + Cy = Vector{NT}(undef, ny*Hp) + Cu = Vector{NT}(undef, nu*Hp) empty = Vector{NT}(undef, 0) - return PredictiveControllerBuffer{NT}(u, R̂y, D̂, empty) + return PredictiveControllerBuffer{NT}(u, R̂y, D̂, Cy, Cu, empty) end include("controller/construct.jl") From 9cd44396f4902e5ee374b664ac71853503e52b81 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Dec 2024 13:33:52 -0500 Subject: [PATCH 09/10] =?UTF-8?q?-=20added:=20reduce=20allocations=20in=20?= =?UTF-8?q?`PredictiveController`=20with=20new=20buffers=20(bis)=20the=20m?= =?UTF-8?q?atrices=20`buffer.E=CC=83`=20and=20`buffer.S=CC=83`=20are=20add?= =?UTF-8?q?ed=20to=20store=20intermediary=20results?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/execute.jl | 25 +++++++++++++------------ src/controller/explicitmpc.jl | 2 +- src/controller/linmpc.jl | 2 +- src/controller/nonlinmpc.jl | 2 +- src/predictive_control.jl | 10 ++++++++-- 5 files changed, 24 insertions(+), 17 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index fd115588..93b8935b 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -192,33 +192,34 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂ mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0) ŷ, F, q̃, r = mpc.ŷ, mpc.F, mpc.q̃, mpc.r Cy, Cu = mpc.buffer.Cy, mpc.buffer.Cu + M_Hp_Ẽ, L_Hp_S̃ = mpc.buffer.Ẽ, mpc.buffer.S̃ ŷ .= evaloutput(mpc.estim, d) - predictstoch!(mpc, mpc.estim) # init mpc.F with Ŷs for InternalModel - F .+= mpc.B - mul!(F, mpc.K, mpc.estim.x̂0, 1, 1) - mul!(F, mpc.V, mpc.estim.lastu0, 1, 1) + predictstoch!(mpc, mpc.estim) # init F with Ŷs for InternalModel + F .+= mpc.B # F = F + B + mul!(F, mpc.K, mpc.estim.x̂0, 1, 1) # F = F + K*x̂0 + mul!(F, mpc.V, mpc.estim.lastu0, 1, 1) # F = F + V*lastu0 if model.nd ≠ 0 mpc.d0 .= d .- model.dop mpc.D̂0 .= D̂ .- mpc.Dop mpc.D̂e[1:model.nd] .= d mpc.D̂e[model.nd+1:end] .= D̂ - mul!(F, mpc.G, mpc.d0, 1, 1) - mul!(F, mpc.J, mpc.D̂0, 1, 1) + mul!(F, mpc.G, mpc.d0, 1, 1) # F = F + G*d0 + mul!(F, mpc.J, mpc.D̂0, 1, 1) # F = F + J*D̂0 end # --- output setpoint tracking term --- mpc.R̂y .= R̂y Cy .= F .- (R̂y .- mpc.Yop) - M_Hp_Ẽ = mpc.weights.M_Hp*mpc.Ẽ - mul!(q̃, M_Hp_Ẽ', Cy) # q̃ = M_Hp*Ẽ'*Cy + mul!(M_Hp_Ẽ, mpc.weights.M_Hp, mpc.Ẽ) + mul!(q̃, M_Hp_Ẽ', Cy) # q̃ = M_Hp*Ẽ'*Cy r .= dot(Cy, mpc.weights.M_Hp, Cy) # --- input setpoint tracking term --- mpc.R̂u .= R̂u Cu .= mpc.T_lastu0 .- (R̂u .- mpc.Uop) - L_Hp_S̃ = mpc.weights.L_Hp*mpc.S̃ - mul!(q̃, L_Hp_S̃', Cu, 1, 1) # q̃ = q̃ + L_Hp*S̃'*Cu + mul!(L_Hp_S̃, mpc.weights.L_Hp, mpc.S̃) + mul!(q̃, L_Hp_S̃', Cu, 1, 1) # q̃ = q̃ + L_Hp*S̃'*Cu r .+= dot(Cu, mpc.weights.L_Hp, Cu) # --- finalize --- - lmul!(2, q̃) # q̃ = 2*q̃ + lmul!(2, q̃) # q̃ = 2*q̃ return nothing end @@ -230,7 +231,7 @@ Init `ŷ, F, d0, D̂0, D̂e, R̂y, R̂u` vectors when model is not a [`LinModel function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u) mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0) mpc.ŷ .= evaloutput(mpc.estim, d) - predictstoch!(mpc, mpc.estim) # init mpc.F with Ŷs for InternalModel + predictstoch!(mpc, mpc.estim) # init F with Ŷs for InternalModel if model.nd ≠ 0 mpc.d0 .= d .- model.dop mpc.D̂0 .= D̂ .- mpc.Dop diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 979123b5..1941126a 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -60,7 +60,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) nΔŨ = size(Ẽ, 2) ΔŨ = zeros(NT, nΔŨ) - buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp) + buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ) mpc = new{NT, SE}( estim, ΔŨ, ŷ, diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 212e100a..1be5895e 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -65,7 +65,7 @@ struct LinMPC{ Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) nΔŨ = size(Ẽ, 2) ΔŨ = zeros(NT, nΔŨ) - buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp) + buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ) mpc = new{NT, SE, JM}( estim, optim, con, ΔŨ, ŷ, diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 07556b0c..ec365ad0 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -79,7 +79,7 @@ struct NonLinMPC{ test_custom_functions(NT, model, JE, gc!, nc, Uop, Yop, Dop, p) nΔŨ = size(Ẽ, 2) ΔŨ = zeros(NT, nΔŨ) - buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp) + buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ) mpc = new{NT, SE, JM, JEfunc, GCfunc, P}( estim, optim, con, ΔŨ, ŷ, diff --git a/src/predictive_control.jl b/src/predictive_control.jl index 1c0afa13..e214c5b3 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -25,6 +25,8 @@ struct PredictiveControllerBuffer{NT<:Real} D̂ ::Vector{NT} Cy::Vector{NT} Cu::Vector{NT} + Ẽ ::Matrix{NT} + S̃ ::Matrix{NT} empty::Vector{NT} end @@ -35,14 +37,18 @@ Create a buffer for `PredictiveController` objects. The buffer is used to store intermediate results during computation without allocating. """ -function PredictiveControllerBuffer{NT}(nu::Int, ny::Int, nd::Int, Hp::Int) where NT <: Real +function PredictiveControllerBuffer{NT}( + nu::Int, ny::Int, nd::Int, Hp::Int, Hc::Int, nϵ::Int +) where NT <: Real u = Vector{NT}(undef, nu) R̂y = Vector{NT}(undef, ny*Hp) D̂ = Vector{NT}(undef, nd*Hp) Cy = Vector{NT}(undef, ny*Hp) Cu = Vector{NT}(undef, nu*Hp) + Ẽ = Matrix{NT}(undef, ny*Hp, nu*Hc + nϵ) + S̃ = Matrix{NT}(undef, nu*Hp, nu*Hc + nϵ) empty = Vector{NT}(undef, 0) - return PredictiveControllerBuffer{NT}(u, R̂y, D̂, Cy, Cu, empty) + return PredictiveControllerBuffer{NT}(u, R̂y, D̂, Cy, Cu, Ẽ, S̃, empty) end include("controller/construct.jl") From 887bcb25a7181928bba665a29bff4274fd2f86c3 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Dec 2024 13:45:29 -0500 Subject: [PATCH 10/10] =?UTF-8?q?changed:=20rename=20`buffer.Cy`=20and=20`?= =?UTF-8?q?buffer.Cy`=20to=20`buffer.Y=CC=82`=20and=20`buffer.U`,=20respec?= =?UTF-8?q?t.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/execute.jl | 3 +-- src/predictive_control.jl | 10 +++++----- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 93b8935b..d113641c 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -191,8 +191,7 @@ They are computed with these equations using in-place operations: function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u) mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0) ŷ, F, q̃, r = mpc.ŷ, mpc.F, mpc.q̃, mpc.r - Cy, Cu = mpc.buffer.Cy, mpc.buffer.Cu - M_Hp_Ẽ, L_Hp_S̃ = mpc.buffer.Ẽ, mpc.buffer.S̃ + Cy, Cu, M_Hp_Ẽ, L_Hp_S̃ = mpc.buffer.Ŷ, mpc.buffer.U, mpc.buffer.Ẽ, mpc.buffer.S̃ ŷ .= evaloutput(mpc.estim, d) predictstoch!(mpc, mpc.estim) # init F with Ŷs for InternalModel F .+= mpc.B # F = F + B diff --git a/src/predictive_control.jl b/src/predictive_control.jl index e214c5b3..f10772da 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -23,8 +23,8 @@ struct PredictiveControllerBuffer{NT<:Real} u ::Vector{NT} R̂y::Vector{NT} D̂ ::Vector{NT} - Cy::Vector{NT} - Cu::Vector{NT} + Ŷ ::Vector{NT} + U ::Vector{NT} Ẽ ::Matrix{NT} S̃ ::Matrix{NT} empty::Vector{NT} @@ -43,12 +43,12 @@ function PredictiveControllerBuffer{NT}( u = Vector{NT}(undef, nu) R̂y = Vector{NT}(undef, ny*Hp) D̂ = Vector{NT}(undef, nd*Hp) - Cy = Vector{NT}(undef, ny*Hp) - Cu = Vector{NT}(undef, nu*Hp) + Ŷ = Vector{NT}(undef, ny*Hp) + U = Vector{NT}(undef, nu*Hp) Ẽ = Matrix{NT}(undef, ny*Hp, nu*Hc + nϵ) S̃ = Matrix{NT}(undef, nu*Hp, nu*Hc + nϵ) empty = Vector{NT}(undef, 0) - return PredictiveControllerBuffer{NT}(u, R̂y, D̂, Cy, Cu, Ẽ, S̃, empty) + return PredictiveControllerBuffer{NT}(u, R̂y, D̂, Ŷ, U, Ẽ, S̃, empty) end include("controller/construct.jl")