Fix type instability in LinMPC and NonLinMPC (introduced in 1.1.0)

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

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
@@ -45,9 +46,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)
@@ -75,7 +84,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ϵ,
@@ -312,9 +321,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),
@@ -323,14 +332,15 @@ function NonLinMPC(
     SE<:StateEstimator{NT}, 
     JM<:JuMP.GenericModel, 
     JEfunc<:Function,
+    GCfunc<:Function,
     P<:Any
 }
     nk = estimate_delays(estim.model)
     if Hp ≤ nk
         @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
@@ -514,7 +524,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
@@ -533,7 +543,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