debug : restore ymin and ymax default value to Inf

franckgaga · franckgaga · commit cf50ced8984c · 2023-05-02T13:42:33.000-04:00
diff --git a/Project.toml b/Project.toml
@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "0.4.0"
+version = "0.4.1"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl
@@ -279,12 +279,16 @@ 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)
-    C_Ŷmin = (mpc.con.Ŷmin - Ŷ)
-    C_Ŷmax = (Ŷ - mpc.con.Ŷmax)
+    # 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 = C_Ŷmin - ϵ*mpc.con.c_Ŷmin
-        C_Ŷmax = C_Ŷmax - ϵ*mpc.con.c_Ŷmin
+        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]
+    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]
     end
     C = [C_Ŷmin; C_Ŷmax]
     return C
diff --git a/src/predictive_control.jl b/src/predictive_control.jl
@@ -633,7 +633,7 @@ end
 obj_quadprog(ΔŨ, P̃, q̃) = 0.5*ΔŨ'*P̃*ΔŨ + q̃'*ΔŨ
 
 """
-    initdefaultcon(model, C, S_Hp, S_Hc, N_Hc, E)
+    init_defaultcon(model, C, S_Hp, S_Hc, N_Hc, E)
 
 Init `ControllerConstraint` struct with default parameters.
 
@@ -643,7 +643,7 @@ function init_defaultcon(model, Hp, Hc, C, S_Hp, S_Hc, N_Hc, E)
     nu, ny = model.nu, model.ny
     umin,       umax    = fill(-Inf, nu), fill(+Inf, nu)
     Δumin,      Δumax   = fill(-Inf, nu), fill(+Inf, nu)
-    ŷmin,       ŷmax    = fill(-999, ny), fill(+999, ny)
+    ŷmin,       ŷmax    = fill(-Inf, ny), fill(+Inf, ny)
     c_umin,     c_umax  = fill(0.0, nu),  fill(0.0, nu)
     c_Δumin,    c_Δumax = fill(0.0, nu),  fill(0.0, nu)
     c_ŷmin,     c_ŷmax  = fill(1.0, ny),  fill(1.0, ny)
