ExplicitMPC seems fully functional

Author: franckgaga

docs/src/public/predictive_control.md

@@ -66,6 +66,12 @@ PredictiveController
 LinMPC
 ```
 
+## ExplicitMPC
+
+```@docs
+ExplicitMPC
+```
+
 ## NonLinMPC
 
 ```@docs

src/controller/explicitmpc.jl

@@ -10,6 +10,8 @@ struct ExplicitMPC{S<:StateEstimator} <: PredictiveController
     M_Hp::Diagonal{Float64, Vector{Float64}}
     Ñ_Hc::Diagonal{Float64, Vector{Float64}}
     L_Hp::Diagonal{Float64, Vector{Float64}}
+    C::Float64
+    E::Float64
     R̂u::Vector{Float64}
     R̂y::Vector{Float64}
     S̃_Hp::Matrix{Bool}
@@ -34,16 +36,19 @@ struct ExplicitMPC{S<:StateEstimator} <: PredictiveController
         model = estim.model
         nu, nxd, nxs, ny, nd = model.nu, model.nx, estim.nxs, model.ny, model.nd
         x̂d, x̂s, ŷ, Ŷs = zeros(nxd), zeros(nxs), zeros(ny), zeros(ny*Hp)
-        validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Inf, ru)
+        Cwt = Inf # no slack variable ϵ for ExplicitMPC
+        Ewt = 0   # economic costs not supported for ExplicitMPC
+        validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru)
         M_Hp = Diagonal{Float64}(repeat(Mwt, Hp))
         N_Hc = Diagonal{Float64}(repeat(Nwt, Hc)) 
         L_Hp = Diagonal{Float64}(repeat(Lwt, Hp))
+        C = Cwt
         # manipulated input setpoint predictions are constant over Hp :
         R̂u = ~iszero(Lwt) ? repeat(ru, Hp) : R̂u = Float64[]
         R̂y = zeros(ny* Hp) # dummy R̂y (updated just before optimization)
         S_Hp, T_Hp, S_Hc, T_Hc = init_ΔUtoU(nu, Hp, Hc)
         E, F, G, J, Kd, Q = init_deterpred(model, Hp, Hc)
-        _ , S̃_Hp, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, Inf, S_Hp, S_Hc, N_Hc, E)
+        _ , S̃_Hp, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, C, S_Hp, S_Hc, N_Hc, E)
         P̃, q̃, p = init_quadprog(model, Ẽ, S̃_Hp, M_Hp, Ñ_Hc, L_Hp)
         Ks, Ps = init_stochpred(estim, Hp)
         d, D̂ = zeros(nd), zeros(nd*Hp)
@@ -54,7 +59,7 @@ struct ExplicitMPC{S<:StateEstimator} <: PredictiveController
             estim,
             ΔŨ, x̂d, x̂s, ŷ, Ŷs,
             Hp, Hc, 
-            M_Hp, Ñ_Hc, L_Hp, R̂u, R̂y,
+            M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, R̂u, R̂y,
             S̃_Hp, T_Hp, T_Hc, 
             Ẽ, F, G, J, Kd, Q, P̃, q̃, p,
             Ks, Ps,
@@ -87,15 +92,14 @@ arguments.
 - `Mwt=fill(1.0,model.ny)` : main diagonal of ``\mathbf{M}`` weight matrix (vector).
 - `Nwt=fill(0.1,model.nu)` : main diagonal of ``\mathbf{N}`` weight matrix (vector).
 - `Lwt=fill(0.0,model.nu)` : main diagonal of ``\mathbf{L}`` weight matrix (vector).
-- `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only.
 - `ru=model.uop` : manipulated input setpoints ``\mathbf{r_u}`` (vector).
 
 # Examples
 ```jldoctest
 julia> model = LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 4);
 
 julia> mpc = ExplicitMPC(model, Mwt=[0, 1], Nwt=[0.5], Hp=30, Hc=1)
-ExplicitMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, SteadyKalmanFilter estimator and:
+ExplicitMPC controller with a sample time Ts = 4.0 s, SteadyKalmanFilter estimator and:
  30 prediction steps Hp
   1 control steps Hc
   1 manipulated inputs u
@@ -120,7 +124,7 @@ Use custom state estimator `estim` to construct `ExplicitMPC`.
 julia> estim = KalmanFilter(LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 4), i_ym=[2]);
 
 julia> mpc = ExplicitMPC(estim, Mwt=[0, 1], Nwt=[0.5], Hp=30, Hc=1)
-ExplicitMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, KalmanFilter estimator and:
+ExplicitMPC controller with a sample time Ts = 4.0 s, KalmanFilter estimator and:
  30 prediction steps Hp
   1 control steps Hc
   1 manipulated inputs u
@@ -152,6 +156,8 @@ function ExplicitMPC(
     return ExplicitMPC{S}(estim, Hp, Hc, Mwt, Nwt, Lwt, ru)
 end
 
+setconstraint!(::ExplicitMPC,kwargs...) = error("ExplicitMPC does not support constraints.")
+
 function Base.show(io::IO, mpc::ExplicitMPC)
     Hp, Hc = mpc.Hp, mpc.Hc
     nu, nd = mpc.estim.model.nu, mpc.estim.model.nd
@@ -173,4 +179,7 @@ Analytically solve the optimization problem for [`ExplicitMPC`](@ref).
 function optim_objective!(mpc::ExplicitMPC)
     mpc.ΔŨ[:] = -mpc.P̃\mpc.q̃
     return mpc.ΔŨ
-end
+end
+
+"For [`ExplicitMPC`](@ref), return an empty summary."
+get_summary(::ExplicitMPC) = solution_summary(JuMP.Model(), verbose=true)

src/controller/linmpc.jl

@@ -13,6 +13,7 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
     Ñ_Hc::Diagonal{Float64, Vector{Float64}}
     L_Hp::Diagonal{Float64, Vector{Float64}}
     C::Float64
+    E::Float64
     R̂u::Vector{Float64}
     R̂y::Vector{Float64}
     S̃_Hp::Matrix{Bool}
@@ -37,6 +38,7 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
         model = estim.model
         nu, nxd, nxs, ny, nd = model.nu, model.nx, estim.nxs, model.ny, model.nd
         x̂d, x̂s, ŷ, Ŷs = zeros(nxd), zeros(nxs), zeros(ny), zeros(ny*Hp)
+        Ewt = 0   # economic costs not supported for LinMPC
         validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru)
         M_Hp = Diagonal{Float64}(repeat(Mwt, Hp))
         N_Hc = Diagonal{Float64}(repeat(Nwt, Hc)) 
@@ -58,7 +60,7 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
             estim, optim, con,
             ΔŨ, x̂d, x̂s, ŷ, Ŷs,
             Hp, Hc, 
-            M_Hp, Ñ_Hc, L_Hp, Cwt, R̂u, R̂y,
+            M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, R̂u, R̂y,
             S̃_Hp, T_Hp, T_Hc, 
             Ẽ, F, G, J, Kd, Q, P̃, q̃, p,
             Ks, Ps,

src/controller/nonlinmpc.jl

@@ -321,12 +321,14 @@ function setnonlincon!(mpc::NonLinMPC, ::NonLinModel)
     return nothing
 end
 
+# TODO: déplacer les 2 prochaines méthodes dans predictive_control.jl
+
 """
-    obj_nonlinprog(mpc::NonLinMPC, model::LinModel, ΔŨ::Vector{Real})
+    obj_nonlinprog(mpc::PredictiveController, model::LinModel, ΔŨ::Vector{Real})
 
 Objective function for [`NonLinMPC`](@ref) when `model` is a [`LinModel`](@ref).
 """
-function obj_nonlinprog(mpc::NonLinMPC, model::LinModel, Ŷ, ΔŨ::Vector{T}) where {T<:Real}
+function obj_nonlinprog(mpc::PredictiveController, model::LinModel, Ŷ, ΔŨ::Vector{T}) where {T<:Real}
     J = obj_quadprog(ΔŨ, mpc.P̃, mpc.q̃)
     if !iszero(mpc.E)
         U = mpc.S̃_Hp*ΔŨ + mpc.T_Hp*(mpc.estim.lastu0 + model.uop)
@@ -339,11 +341,11 @@ function obj_nonlinprog(mpc::NonLinMPC, model::LinModel, Ŷ, ΔŨ::Vector{T})
 end
 
 """
-    obj_nonlinprog(mpc::NonLinMPC, model::SimModel, ΔŨ::Vector{Real})
+    obj_nonlinprog(mpc::PredictiveController, model::SimModel, ΔŨ::Vector{Real})
 
 Objective function for [`NonLinMPC`](@ref) when `model` is not a [`LinModel`](@ref).
 """
-function obj_nonlinprog(mpc::NonLinMPC, model::SimModel, Ŷ, ΔŨ::Vector{T}) where {T<:Real}
+function obj_nonlinprog(mpc::PredictiveController, model::SimModel, Ŷ, ΔŨ::Vector{T}) where {T<:Real}
     # --- output setpoint tracking term ---
     êy = mpc.R̂y - Ŷ
     JR̂y = êy'*mpc.M_Hp*êy  

src/predictive_control.jl

@@ -303,23 +303,30 @@ julia> sol_summary, info = getinfo(mpc); round.(info[:Ŷ], digits=3)
 ```
 """
 function getinfo(mpc::PredictiveController)
-    sol_summary = solution_summary(mpc.optim) 
+    sol_summary = get_summary(mpc) 
+    Ŷ = predict(mpc, mpc.estim.model, mpc.ΔŨ)
     info = Dict{Symbol, InfoDictType}()
     info[:ΔU]  = mpc.ΔŨ[1:mpc.Hc*mpc.estim.model.nu]
     info[:ϵ]   = isinf(mpc.C) ? NaN : mpc.ΔŨ[end]
-    info[:J]   = objective_value(mpc.optim) + mpc.p[begin]
+    info[:J]   = obj_nonlinprog(mpc, mpc.estim.model, Ŷ, mpc.ΔŨ) + mpc.p[begin]
     info[:U]   = mpc.S̃_Hp*mpc.ΔŨ + mpc.T_Hp*(mpc.estim.lastu0 + mpc.estim.model.uop)
     info[:u]   = info[:U][1:mpc.estim.model.nu]
     info[:d]   = mpc.d
     info[:D̂]   = mpc.D̂
     info[:ŷ]   = mpc.ŷ
-    info[:Ŷ]   = predict(mpc, mpc.estim.model, mpc.ΔŨ)
+    info[:Ŷ]   = Ŷ
     info[:Ŷs]  = mpc.Ŷs
     info[:Ŷd]  = info[:Ŷ] - info[:Ŷs]
     info[:R̂y]  = mpc.R̂y
     info[:R̂u]  = mpc.R̂u
     return sol_summary, info
 end
+"""
+    get_summary(mpc::PredictiveController)
+
+Get optimizer solution summary `sol_summary`. 
+"""
+get_summary(mpc::PredictiveController) = solution_summary(mpc.optim, verbose=true)
 
 """
     setstate!(mpc::PredictiveController, x̂)
@@ -333,7 +340,7 @@ setstate!(mpc::PredictiveController, x̂) = (setstate!(mpc.estim, x̂); return m
 
 Init `mpc.ΔŨ` for warm-starting and the states of `mpc.estim` [`StateEstimator`](@ref).
 
-Before calling [`initstate!(::StateEstimator, _ , _ )`](@ref), it warm-starts ``\mathbf{ΔŨ}``:
+Before calling [`initstate!(::StateEstimator,_,_)`](@ref), it warm-starts ``\mathbf{ΔŨ}``:
 - If `model` is a [`LinModel`], the vector is filled with the analytical minimum ``J`` of
   the unconstrained problem.
 - Else, the vector is filled with zeros.

test/test_predictive_control.jl

@@ -85,6 +85,109 @@ end
     @test mpc1.estim.x̂ ≈ [0,0,0,0]
     @test_throws ArgumentError updatestate!(mpc1, [0,0])
 end
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+@testset "ExplicitMPC construction" begin
+    model = LinModel(sys, Ts, i_d=[3])
+    mpc1 = ExplicitMPC(model, Hp=15)
+    @test isa(mpc1.estim, SteadyKalmanFilter)
+    @test size(mpc1.Ẽ,1) == 15*mpc1.estim.model.ny
+    mpc4 = ExplicitMPC(model, Mwt=[1,2], Hp=15)
+    @test mpc4.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15)))
+    mpc5 = ExplicitMPC(model, Nwt=[3,4], Hc=5)
+    @test mpc5.Ñ_Hc ≈ Diagonal(diagm(repeat(Float64[3, 4], 5)))
+    mpc6 = ExplicitMPC(model, Lwt=[0,1], ru=[0,50], Hp=15)
+    @test mpc6.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15)))
+    @test mpc6.R̂u ≈ repeat([0,50], 15)
+    kf = KalmanFilter(model)
+    mpc8 = ExplicitMPC(kf)
+    @test isa(mpc8.estim, KalmanFilter)
+
+    @test_throws ErrorException ExplicitMPC(model, Hp=0)
+    @test_throws ErrorException ExplicitMPC(model, Hc=0)
+    @test_throws ErrorException ExplicitMPC(model, Hp=1, Hc=2)
+    @test_throws ErrorException ExplicitMPC(model, Mwt=[1])
+    @test_throws ErrorException ExplicitMPC(model, Mwt=[1])
+    @test_throws ErrorException ExplicitMPC(model, Lwt=[1])
+    @test_throws ErrorException ExplicitMPC(model, ru=[1])
+    @test_throws ErrorException ExplicitMPC(model, Mwt=[-1,1])
+    @test_throws ErrorException ExplicitMPC(model, Nwt=[-1,1])
+    @test_throws ErrorException ExplicitMPC(model, Lwt=[-1,1])
+end
+
+@testset "ExplicitMPC constraints" begin
+    model = LinModel(sys, Ts, i_d=[3])
+    mpc = ExplicitMPC(model, Hp=1, Hc=1)
+    @test_throws ErrorException setconstraint!(mpc, umin=[0.0, 0.0])
+end
+
+@testset "ExplicitMPC moves and getinfo" begin
+    mpc1 = ExplicitMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1)
+    r = [5]
+    u = moveinput!(mpc1, r)
+    @test u ≈ [1] atol=1e-2
+    u = mpc1(r)
+    @test u ≈ [1] atol=1e-2
+    _ , info = getinfo(mpc1)
+    @test info[:u] ≈ u
+    @test info[:Ŷ][end] ≈ r[1] atol=1e-2
+    mpc2 = ExplicitMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1)
+    u = moveinput!(mpc2, [5])
+    @test u ≈ [1] atol=1e-2
+    mpc3 = ExplicitMPC(LinModel(tf(5, [2, 1]), 3), Mwt=[0], Nwt=[0], Lwt=[1], ru=[12])
+    u = moveinput!(mpc3, [0])
+    @test u ≈ [12] atol=1e-2
+end
+
+@testset "ExplicitMPC other methods" begin
+    linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
+    mpc1 = ExplicitMPC(linmodel1)
+    @test initstate!(mpc1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]]
+    setstate!(mpc1, [1,2,3,4])
+    @test mpc1.estim.x̂ ≈ [1,2,3,4]
+    setstate!(mpc1, [0,0,0,0])
+    updatestate!(mpc1, mpc1.estim.model.uop, mpc1.estim())
+    @test mpc1.estim.x̂ ≈ [0,0,0,0]
+    @test_throws ArgumentError updatestate!(mpc1, [0,0])
+end
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
 
 @testset "NonLinMPC construction" begin
     linmodel1 = LinModel(sys,Ts,i_d=[3])