added: MultipleShooting with NonLinModel is working 🍾

Author: franckgaga

src/controller/construct.jl

@@ -114,6 +114,8 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}}
     Aeq     ::Matrix{NT}
     # b vector for the linear equality constraints:
     beq     ::Vector{NT}
+    # nonlinear equality constraints:
+    neq     ::Int
     # constraint softness parameter vectors for the nonlinear inequality constraints:
     C_ymin  ::Vector{NT}
     C_ymax  ::Vector{NT}
@@ -404,9 +406,9 @@ end
         model::LinModel, transcription::TranscriptionMethod, nc::Int,
         i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, 
         args...
-    ) -> i_b, i_g, A, Aeq
+    ) -> i_b, i_g, A, Aeq, neq
 
-Init `i_b`, `i_g`, `A` and `Aeq` matrices for the linear and nonlinear constraints.
+Init `i_b`, `i_g`, `neq`, and `A` and `Aeq` matrices for the all the MPC constraints.
 
 The linear and nonlinear constraints are respectively defined as:
 ```math
@@ -420,25 +422,27 @@ The linear and nonlinear constraints are respectively defined as:
 The argument `nc` is the number of custom nonlinear inequality constraints in
 ``\mathbf{g_c}``. `i_b` is a `BitVector` including the indices of ``\mathbf{b}`` that are
 finite numbers. `i_g` is a similar vector but for the indices of ``\mathbf{g}``. The method
-also returns the ``\mathbf{A, A_{eq}}`` matrices if `args` is provided. In such a case,
-`args`  needs to contain all the inequality and equality constraint matrices: 
-`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ`.
+also returns the ``\mathbf{A, A_{eq}}`` matrices and `neq` if `args` is provided. In such a 
+case, `args`  needs to contain all the inequality and equality constraint matrices: 
+`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ`. The integer `neq`
+is the number of nonlinear equality constraints in ``\mathbf{g_{eq}}``.
 """
 function init_matconstraint_mpc(
     ::LinModel{NT}, ::TranscriptionMethod, nc::Int,
     i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, 
     args...
 ) where {NT<:Real}
     if isempty(args)
-        A, Aeq = nothing, nothing
+        A, Aeq, neq = nothing, nothing, nothing
     else
         A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ = args
         A   = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ymin; A_Ymax; A_x̂min; A_x̂max]
         Aeq = A_ŝ
+        neq = 0
     end
     i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Ymin; i_Ymax; i_x̂min; i_x̂max]
     i_g = trues(nc)
-    return i_b, i_g, A, Aeq
+    return i_b, i_g, A, Aeq, neq
 end
 
 "Init `i_b` without output constraints if [`NonLinModel`](@ref) & [`MultipleShooting`](@ref)."
@@ -448,15 +452,17 @@ function init_matconstraint_mpc(
     args...
 ) where {NT<:Real}
     if isempty(args)
-        A, Aeq = nothing, nothing
+        A, Aeq, neq = nothing, nothing, nothing
     else
         A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ = args
         A   = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ymin; A_Ymax; A_x̂min; A_x̂max]
         Aeq = A_ŝ
+        nΔŨ, nZ̃ = size(A_ΔŨmin)
+        neq = nZ̃ - nΔŨ
     end
     i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_x̂min; i_x̂max]
     i_g = [i_Ymin; i_Ymax; trues(nc)]
-    return i_b, i_g, A, Aeq
+    return i_b, i_g, A, Aeq, neq
 end
 
 "Init `i_b` without output & terminal constraints if [`NonLinModel`](@ref) & [`SingleShooting`](@ref)."
@@ -466,15 +472,16 @@ function init_matconstraint_mpc(
     args...
 ) where {NT<:Real}
     if isempty(args)
-        A, Aeq = nothing, nothing
+        A, Aeq, neq = nothing, nothing, nothing
     else
         A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ = args
         A   = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ymin; A_Ymax; A_x̂min; A_x̂max]
         Aeq = A_ŝ
+        neq = 0
     end
     i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax]
     i_g = [i_Ymin; i_Ymax; i_x̂min;  i_x̂max; trues(nc)]
-    return i_b, i_g, A, Aeq
+    return i_b, i_g, A, Aeq, neq
 end
 
 
@@ -603,7 +610,7 @@ function init_defaultcon_mpc(
     i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax)
     i_Ymin,  i_Ymax  = .!isinf.(Y0min), .!isinf.(Y0max)
     i_x̂min,  i_x̂max  = .!isinf.(x̂0min), .!isinf.(x̂0max)
-    i_b, i_g, A, Aeq = init_matconstraint_mpc(
+    i_b, i_g, A, Aeq, neq = init_matconstraint_mpc(
         model, transcription, nc,
         i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max,
         A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂max, A_x̂min,
@@ -619,6 +626,7 @@ function init_defaultcon_mpc(
         A       , b      , i_b    , 
         A_ŝ     ,
         Aeq     , beq    ,
+        neq     ,
         C_ymin  , C_ymax , c_x̂min , c_x̂max , i_g,
         gc!     , nc
     )

src/controller/execute.jl

@@ -330,44 +330,36 @@ function linconstraint!(mpc::PredictiveController, ::SimModel)
 end
 
 """
-    nonlinprog_vectors!(ΔŨ, Ŷe, Ue, Ū, mpc::PredictiveController, Ŷ0, Z̃) -> ΔŨ, Ŷe, Ue
+    extended_vectors!(Ue, Ŷe, mpc::PredictiveController, U0, Ŷ0) -> Ue, Ŷe
 
-Compute the `ΔŨ`, `Ŷe` and `Ue` vectors for nonlinear programming using `Ŷ0` and `Z̃`.
+Compute the extended `Ue` and `Ŷe` vectors for nonlinear programming using `U0` and `Ŷ0`.
 
-See [`NonLinMPC`](@ref) for the definition of the vectors. The function mutates `ΔŨ`, `Ŷe`,
-`Ue` and `Ū` in arguments, without assuming any initial values for them. Using 
+See [`NonLinMPC`](@ref) for the definition of the vectors. The function mutates `Ue` and
+and `Ŷe` in arguments, without assuming any initial values for them. Using 
 `nocustomfcts = mpc.weights.iszero_E && mpc.con.nc == 0`, there are three special cases in
-which ΔŨ, `Ŷe`, `Ue` and `Ū` are not mutated:
+which `Ue` and `Ŷe` are not mutated:
 
-- If `mpc.weights.iszero_Ñ_Hc[]`, the `ΔŨ` vector is not computed to reduce the burden in 
-  the optimization problem.
 - If `mpc.weights.iszero_M_Hp[] && nocustomfcts`, the `Ŷe` vector is not computed for the
   same reason as above.
 - If `mpc.weights.iszero_L_Hp[] && nocustomfcts`, the `Ue` vector is not computed for the
   same reason as above.
 """
-function nonlinprog_vectors!(ΔŨ, Ŷe, Ue, Ū, mpc::PredictiveController, Ŷ0, Z̃)
+function extended_vectors!(Ue, Ŷe, mpc::PredictiveController, U0, Ŷ0)
     model = mpc.estim.model
     ny, nu = model.ny, model.nu
     nocustomfcts = (mpc.weights.iszero_E && iszero_nc(mpc))
-    # --- augmented input increments ΔŨ = [ΔU; ϵ] ---
-    if !(mpc.weights.iszero_Ñ_Hc[])
-        ΔŨ .= mul!(ΔŨ, mpc.P̃Δu, Z̃)
+    # --- extended manipulated inputs Ue = [U; u(k+Hp-1)] ---
+    if !(mpc.weights.iszero_L_Hp[] && nocustomfcts)
+        Ue[1:end-nu] .= U0 .+ mpc.Uop
+        # u(k + Hp) = u(k + Hp - 1) since Δu(k+Hp) = 0 (because Hc ≤ Hp):
+        Ue[end-nu+1:end] .= @views U[end-nu+1:end]
     end
     # --- extended output predictions Ŷe = [ŷ(k); Ŷ] ---
     if !(mpc.weights.iszero_M_Hp[] && nocustomfcts)
         Ŷe[1:ny] .= mpc.ŷ
         Ŷe[ny+1:end] .= Ŷ0 .+ mpc.Yop
     end
-    # --- extended manipulated inputs Ue = [U; u(k+Hp-1)] ---
-    if !(mpc.weights.iszero_L_Hp[] && nocustomfcts)
-        U  = Ū
-        U .= mul!(U, mpc.P̃u, Z̃) .+ mpc.Tu_lastu
-        Ue[1:end-nu] .= U
-        # u(k + Hp) = u(k + Hp - 1) since Δu(k+Hp) = 0 (because Hc ≤ Hp):
-        Ue[end-nu+1:end] .= @views U[end-nu+1:end]
-    end
-    return ΔŨ, Ŷe, Ue 
+    return Ue, Ŷe 
 end
 
 "Verify if the custom nonlinear constraint has zero elements."
@@ -391,23 +383,23 @@ function obj_nonlinprog!(
 end
 
 """
-    obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ, Z̃)
+    obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ)
 
 Nonlinear programming objective method when `model` is not a [`LinModel`](@ref). The
 function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. This method mutates
 `Ȳ` and `Ū` arguments, without assuming any initial values (it recuperates the values in
 `Ŷe` and `Ue` arguments).
 """
 function obj_nonlinprog!(
-    Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ, ::AbstractVector{NT}
+    Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ::AbstractVector{NT}
 ) where NT<:Real
     nu, ny = model.nu, model.ny
     # --- output setpoint tracking term ---
     if mpc.weights.iszero_M_Hp[]
         JR̂y = zero(NT)
     else
         Ȳ  .= @views Ŷe[ny+1:end]
-        Ȳ  .= mpc.R̂y .- Ȳ
+        Ȳ  .= Ȳ .- mpc.R̂y  
         JR̂y = dot(Ȳ, mpc.weights.M_Hp, Ȳ)
     end
     # --- move suppression and slack variable term ---
@@ -421,7 +413,7 @@ function obj_nonlinprog!(
         JR̂u = zero(NT)
     else
         Ū  .= @views Ue[1:end-nu]
-        Ū  .= mpc.R̂u .- Ū
+        Ū  .= Ū .- mpc.R̂u
         JR̂u = dot(Ū, mpc.weights.L_Hp, Ū)
     end
     # --- economic term ---