remove i_Ymin and i_Ymax fields in ControllerConstraint

franckgaga · franckgaga · commit 37a3b10a8ca1 · 2023-05-02T14:02:02.000-04:00
diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl
@@ -209,11 +209,11 @@ function setnonlincon!(mpc::NonLinMPC, model::NonLinModel)
     ΔŨ = mpc.optim[:ΔŨ]
     con = mpc.con
     map(con -> delete(optim, con), all_nonlinear_constraints(optim))
-    for i in findall(con.i_Ŷmin)
+    for i in findall(.!isinf.(con.Ŷmin))
         f_sym = Symbol("C_Ŷmin_$(i)")
         add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨ...)) <= 0))
     end
-    for i in findall(con.i_Ŷmax)
+    for i in findall(.!isinf.(con.Ŷmax))
         f_sym = Symbol("C_Ŷmax_$(i)")
         add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨ...)) <= 0))
     end
@@ -279,17 +279,17 @@ function con_nonlinprog(mpc::NonLinMPC, model::SimModel, ΔŨ::NTuple{N, T}) wh
     ΔŨ = collect(ΔŨ) # convert NTuple to Vector
     U0 = mpc.S̃_Hp*ΔŨ + mpc.T_Hp*(mpc.estim.lastu0)
     Ŷ = evalŶ(mpc, model, mpc.x̂d, mpc.d0, mpc.D̂0, U0)
-    # replace -Inf with 0 to avoid INVALID_MODEL error
-    C_Ŷmin = zeros(T, length(Ŷ))
-    C_Ŷmax = zeros(T, length(Ŷ))
     if !isinf(mpc.C) # constraint softening activated :
         ϵ = ΔŨ[end]
-        C_Ŷmin[mpc.con.i_Ŷmin] = (mpc.con.Ŷmin - Ŷ - ϵ*mpc.con.c_Ŷmin)[mpc.con.i_Ŷmin]
-        C_Ŷmax[mpc.con.i_Ŷmax] = (Ŷ - mpc.con.Ŷmax - ϵ*mpc.con.c_Ŷmax)[mpc.con.i_Ŷmax]
+        C_Ŷmin = (mpc.con.Ŷmin - Ŷ) - ϵ*mpc.con.c_Ŷmin
+        C_Ŷmax = (Ŷ - mpc.con.Ŷmax) - ϵ*mpc.con.c_Ŷmax
     else # no constraint softening :
-        C_Ŷmin[mpc.con.i_Ŷmin] = (mpc.con.Ŷmin - Ŷ)[mpc.con.i_Ŷmin]
-        C_Ŷmax[mpc.con.i_Ŷmax] = (Ŷ - mpc.con.Ŷmax)[mpc.con.i_Ŷmax]
+        C_Ŷmin = (mpc.con.Ŷmin - Ŷ)
+        C_Ŷmax = (Ŷ - mpc.con.Ŷmax)
     end
+    # replace -Inf with 0 to avoid INVALID_MODEL error :
+    C_Ŷmin[isinf.(C_Ŷmin)] .= 0
+    C_Ŷmax[isinf.(C_Ŷmax)] .= 0
     C = [C_Ŷmin; C_Ŷmax]
     return C
 end
diff --git a/src/predictive_control.jl b/src/predictive_control.jl
@@ -36,8 +36,6 @@ struct ControllerConstraint
     A      ::Matrix{Float64}
     b      ::Vector{Float64}
     i_b    ::BitVector
-    i_Ŷmin ::BitVector
-    i_Ŷmax ::BitVector
     c_Ŷmin ::Vector{Float64}
     c_Ŷmax ::Vector{Float64}
 end
@@ -117,13 +115,11 @@ function setconstraint!(
         size(ŷmin)   == (ny,) || error("ŷmin size must be $((ny,))")
         Ŷmin  = repeat(ŷmin, Hp)
         con.Ŷmin[:] = Ŷmin
-        con.i_Ŷmin[:] = .!isinf.(Ŷmin)
     end
     if !isnothing(ŷmax)
         size(ŷmax)   == (ny,) || error("ŷmax size must be $((ny,))")
         Ŷmax  = repeat(ŷmax, Hp)
         con.Ŷmax[:] = Ŷmax
-        con.i_Ŷmax[:] = .!isinf.(Ŷmax)
     end
     if !isnothing(c_umin)
         !isinf(C) || error("Slack variable Cwt must be finite to set softness parameters") 
@@ -665,7 +661,7 @@ function init_defaultcon(model, Hp, Hc, C, S_Hp, S_Hc, N_Hc, E)
     con = ControllerConstraint(
         Umin    , Umax  , ΔŨmin  , ΔŨmax    , Ŷmin  , Ŷmax,
         A_Umin  , A_Umax, A_ΔŨmin, A_ΔŨmax  , A_Ŷmin, A_Ŷmax,
-        A       , b     , i_b  , i_Ŷmin, i_Ŷmax, c_Ŷmin, c_Ŷmax 
+        A       , b     , i_b    , c_Ŷmin   , c_Ŷmax 
     )
     return con, S̃_Hp, Ñ_Hc, Ẽ
 end
