function def order modification

franckgaga · franckgaga · commit 61e8c45aead6 · 2023-04-27T09:45:40.000-04:00
diff --git a/src/predictive_control.jl b/src/predictive_control.jl
@@ -42,44 +42,6 @@ struct ControllerConstraint
     c_Ŷmax ::Vector{Float64}
 end
 
-"""
-    initdefaultcon(model, C, S_Hp, S_Hc, N_Hc, E)
-
-Init `ContollerConstraint` struct with default paramaters.
-
-Also return `S̃_Hp`, `Ñ_Hc` and `Ẽ` matrices for the the augmented decision vector `ΔŨ`.
-"""
-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(-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)
-    Umin, Umax, ΔUmin, ΔUmax, Ŷmin, Ŷmax = 
-        repeat_constraints(Hp, Hc, umin, umax, Δumin, Δumax, ŷmin, ŷmax)
-    c_Umin, c_Umax, c_ΔUmin, c_ΔUmax, c_Ŷmin, c_Ŷmax = 
-        repeat_constraints(Hp, Hc, c_umin, c_umax, c_Δumin, c_Δumax, c_ŷmin, c_ŷmax)
-    A_Umin, A_Umax, S̃_Hp = relaxU(C, c_Umin, c_Umax, S_Hp, S_Hc)
-    A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, Ñ_Hc = relaxΔU(C,c_ΔUmin,c_ΔUmax,ΔUmin,ΔUmax,N_Hc)
-    A_Ŷmin, A_Ŷmax, Ẽ = relaxŶ(model, C, c_Ŷmin, c_Ŷmax, E)
-    i_Umin,  i_Umax  = .!isinf.(Umin),  .!isinf.(Umax)
-    i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax)
-    i_Ŷmin,  i_Ŷmax  = .!isinf.(Ŷmin),  .!isinf.(Ŷmax)
-    A, i_b, b = init_linconstraint(
-        model, 
-        A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ŷmin, A_Ŷmax,
-        i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ŷmin, i_Ŷmax
-    )
-    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 
-    )
-    return con, S̃_Hp, Ñ_Hc, Ẽ
-end
-
 @doc raw"""
     setconstraint!(mpc::PredictiveController; <keyword arguments>)
 
@@ -486,18 +448,6 @@ evalŷ!(estim::InternalModel, ym, d) = (mpc.ŷ[:] = estim(ym, d))
 "Evaluate current output of the other `StateEstimator`s."
 evalŷ!(estim::StateEstimator, _, d) = (mpc.ŷ[:] = estim(d))
 
-"Repeat predictive controller constraints over prediction `Hp` and control `Hc` horizons."
-function repeat_constraints(Hp, Hc, umin, umax, Δumin, Δumax, ŷmin, ŷmax)
-    Umin  = repeat(umin, Hc)
-    Umax  = repeat(umax, Hc)
-    ΔUmin = repeat(Δumin, Hc)
-    ΔUmax = repeat(Δumax, Hc)
-    Ŷmin  = repeat(ŷmin, Hp)
-    Ŷmax  = repeat(ŷmax, Hp)
-    return Umin, Umax, ΔUmin, ΔUmax, Ŷmin, Ŷmax
-end
-
-
 @doc raw"""
     init_ΔUtoU(nu, Hp, Hc, C, c_Umin, c_Umax)
 
@@ -676,6 +626,55 @@ end
 "Return the quadratic programming objective function, see [`init_quadprog`](@ref)."
 obj_quadprog(ΔŨ, P̃, q̃) = 0.5*ΔŨ'*P̃*ΔŨ + q̃'*ΔŨ
 
+"""
+    initdefaultcon(model, C, S_Hp, S_Hc, N_Hc, E)
+
+Init `ContollerConstraint` struct with default paramaters.
+
+Also return `S̃_Hp`, `Ñ_Hc` and `Ẽ` matrices for the the augmented decision vector `ΔŨ`.
+"""
+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(-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)
+    Umin, Umax, ΔUmin, ΔUmax, Ŷmin, Ŷmax = 
+        repeat_constraints(Hp, Hc, umin, umax, Δumin, Δumax, ŷmin, ŷmax)
+    c_Umin, c_Umax, c_ΔUmin, c_ΔUmax, c_Ŷmin, c_Ŷmax = 
+        repeat_constraints(Hp, Hc, c_umin, c_umax, c_Δumin, c_Δumax, c_ŷmin, c_ŷmax)
+    A_Umin, A_Umax, S̃_Hp = relaxU(C, c_Umin, c_Umax, S_Hp, S_Hc)
+    A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, Ñ_Hc = relaxΔU(C,c_ΔUmin,c_ΔUmax,ΔUmin,ΔUmax,N_Hc)
+    A_Ŷmin, A_Ŷmax, Ẽ = relaxŶ(model, C, c_Ŷmin, c_Ŷmax, E)
+    i_Umin,  i_Umax  = .!isinf.(Umin),  .!isinf.(Umax)
+    i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax)
+    i_Ŷmin,  i_Ŷmax  = .!isinf.(Ŷmin),  .!isinf.(Ŷmax)
+    A, i_b, b = init_linconstraint(
+        model, 
+        A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ŷmin, A_Ŷmax,
+        i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ŷmin, i_Ŷmax
+    )
+    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 
+    )
+    return con, S̃_Hp, Ñ_Hc, Ẽ
+end
+
+"Repeat predictive controller constraints over prediction `Hp` and control `Hc` horizons."
+function repeat_constraints(Hp, Hc, umin, umax, Δumin, Δumax, ŷmin, ŷmax)
+    Umin  = repeat(umin, Hc)
+    Umax  = repeat(umax, Hc)
+    ΔUmin = repeat(Δumin, Hc)
+    ΔUmax = repeat(Δumax, Hc)
+    Ŷmin  = repeat(ŷmin, Hp)
+    Ŷmax  = repeat(ŷmax, Hp)
+    return Umin, Umax, ΔUmin, ΔUmax, Ŷmin, Ŷmax
+end
+
 @doc raw"""
     relaxU(C, c_Umin, c_Umax, S_Hp, S_Hc)
 
