debug: terminal constr. for NonLinMPC now work

- added : terminal constraint tests for `NonLinMPC`
- added : reduce allocation for `NonLinMPC` based on `NonLinModel`

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "0.10.2"
+version = "0.10.3"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

docs/src/internals/predictive_control.md

@@ -16,7 +16,7 @@ ModelPredictiveControl.init_predmat
 ModelPredictiveControl.init_ΔUtoU
 ModelPredictiveControl.init_quadprog
 ModelPredictiveControl.init_stochpred
-ModelPredictiveControl.init_linconstraint
+ModelPredictiveControl.init_matconstraint
 ```
 
 ## Constraint Relaxation

docs/src/manual/linmpc.md

@@ -87,8 +87,8 @@ automatically added to the model outputs by default if observability is preserve
 [`SteadyKalmanFilter`](@ref) for details).
 
 [^1]: As an alternative to state observer, we could have use an [`InternalModel`](@ref)
-    structure with `mpc = LinMPC(InternalModel(model), Hp=15, Hc=2, Mwt=[1, 1], Nwt=[0.1, 0.1])` . It was
-    tested on the example of this page and it gives similar results.
+    structure with `mpc = LinMPC(InternalModel(model), Hp=15, Hc=2, Mwt=[1, 1], Nwt=[0.1, 0.1])`.
+    It was tested on the example of this page and it gives similar results.
 
 Before closing the loop, we call [`initstate!`](@ref) with the actual plant inputs and
 measurements to ensure a bumpless transfer. Since `model` simulates our plant here, its

docs/src/manual/nonlinmpc.md

@@ -56,8 +56,10 @@ nu, nx, ny = 1, 2, 1
 model = NonLinModel(f, h, Ts, nu, nx, ny)
 ```
 
-The output function ``\mathbf{h}`` converts the ``θ`` angle to degrees. It is good practice
-to first simulate `model` using [`sim!`](@ref) as a quick sanity check:
+The output function ``\mathbf{h}`` converts the ``θ`` angle to degrees. Note that special
+characters like ``θ`` can be typed in the Julia REPL or VS Code by typing `\theta` and
+pressing the `<TAB>` key. It is good practice to first simulate `model` using [`sim!`](@ref)
+as a quick sanity check:
 
 ```@example 1
 using Plots

src/controller/nonlinmpc.jl

@@ -258,8 +258,7 @@ function init_optimization!(mpc::NonLinMPC)
     @constraint(optim, linconstraint, A*ΔŨvar .≤ b)
     # --- nonlinear optimization init ---
     model = mpc.estim.model
-    ny, nu, nx̂, Hp = model.ny, model.nu, mpc.estim.nx̂, mpc.Hp
-    nC = (2*Hp*nu + 2*nvar + 2*Hp*ny + 2*nx̂) - length(mpc.con.b)
+    ny, nx̂, Hp, nC = model.ny, mpc.estim.nx̂, mpc.Hp, length(con.i_C)
     # inspired from https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs
     Jfunc, Cfunc = let mpc=mpc, model=model, nC=nC, nvar=nvar , nŶ=Hp*ny, nx̂=nx̂
         last_ΔŨtup_float, last_ΔŨtup_dual = nothing, nothing
@@ -341,11 +340,7 @@ function init_optimization!(mpc::NonLinMPC)
     return nothing
 end
 
-
-"No nonlinear constraint for [`NonLinMPC`](@ref) based on [`LinModel`](@ref)."
-setnontlincon!(::NonLinMPC, ::LinModel) = nothing
-
-"Set the nonlinear constraints on the output predictions `Ŷ`."
+"Set the nonlinear constraints on the output predictions `Ŷ` ans terminal states `x̂end`."
 function setnonlincon!(mpc::NonLinMPC, ::NonLinModel)
     optim = mpc.optim
     ΔŨvar = mpc.optim[:ΔŨvar]
@@ -371,27 +366,31 @@ function setnonlincon!(mpc::NonLinMPC, ::NonLinModel)
 end
 
 """
-    con_nonlinprog!(C, mpc::NonLinMPC, model::SimModel, x̂end, Ŷ, ΔŨ)
+    con_nonlinprog!(C, mpc::NonLinMPC, model::SimModel, x̂end, Ŷ, ΔŨ) -> C
 
 Nonlinear constrains for [`NonLinMPC`](@ref) when `model` is not a [`LinModel`](@ref).
+
+The method mutates the `C` vector in argument and returns it.
 """
 function con_nonlinprog!(C, mpc::NonLinMPC, model::SimModel, x̂end, Ŷ, ΔŨ)
-    ny, nx̂, Hp = model.ny, mpc.estim.nx̂, mpc.Hp
-    i_end_Ymin, i_end_Ymax = 1Hp*ny      , 2Hp*ny
-    i_end_x̂min, i_end_x̂max = 2Hp*ny + 1nx̂, 2Hp*ny + 2nx̂
-    if !isinf(mpc.C) # constraint softening activated :
-        ϵ = ΔŨ[end]
-        C[           1:i_end_Ymin] = (mpc.con.Ymin - Ŷ) - ϵ*mpc.con.c_Ymin
-        C[i_end_Ymin+1:i_end_Ymax] = (Ŷ - mpc.con.Ymax) - ϵ*mpc.con.c_Ymax
-        C[i_end_Ymax+1:i_end_x̂min] = (mpc.con.x̂min - x̂end) - ϵ*mpc.con.c_x̂min
-        C[i_end_x̂min+1:i_end_x̂max] = (x̂end - mpc.con.x̂max) - ϵ*mpc.con.c_x̂max
-    else # no constraint softening :
-        C[           1:i_end_Ymin] = mpc.con.Ymin - Ŷ
-        C[i_end_Ymin+1:i_end_Ymax] = Ŷ - mpc.con.Ymax
-        C[i_end_Ymax+1:i_end_x̂min] = mpc.con.x̂min - x̂end
-        C[i_end_x̂min+1:i_end_x̂max] = x̂end - mpc.con.x̂max
+    nx̂, nŶ = mpc.estim.nx̂, model.ny*mpc.Hp
+    ϵ = !isinf(mpc.C) ? ΔŨ[end] : 0.0 # ϵ = 0.0 if Cwt=Inf (meaning: no relaxation)
+    for i in eachindex(C)
+        mpc.con.i_C[i] || continue
+        if i ≤ nŶ
+            j = i
+            C[i] = (mpc.con.Ymin[j] - Ŷ[j])     - ϵ*mpc.con.c_Ymin[j]
+        elseif i ≤ 2nŶ
+            j = i - nŶ
+            C[i] = (Ŷ[j] - mpc.con.Ymax[j])     - ϵ*mpc.con.c_Ymax[j]
+        elseif i ≤ 2nŶ + nx̂
+            j = i - 2nŶ
+            C[i] = (mpc.con.x̂min[j] - x̂end[j])  - ϵ*mpc.con.c_x̂min[j]
+        else
+            j = i - 2nŶ - nx̂
+            C[i] = (x̂end[j] - mpc.con.x̂max[j])  - ϵ*mpc.con.c_x̂max[j]
+        end
     end
-    C[isinf.(C)] .= 0 # replace ±Inf with 0 to avoid INVALID_MODEL error
     return C
 end
 

src/predictive_control.jl

@@ -67,6 +67,7 @@ struct ControllerConstraint
     c_Ymax  ::Vector{Float64}
     c_x̂min  ::Vector{Float64}
     c_x̂max  ::Vector{Float64}
+    i_C     ::BitVector
 end
 
 @doc raw"""
@@ -306,9 +307,11 @@ function setconstraint!(
     i_Ymin,  i_Ymax  = .!isinf.(con.Ymin),  .!isinf.(con.Ymax)
     i_x̂min,  i_x̂max  = .!isinf.(con.x̂min),  .!isinf.(con.x̂max)
     if notSolvedYet
-        con.i_b[:], con.A[:] = init_linconstraint(model,
-            i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max,
-            con.A_Umin, con.A_Umax, con.A_ΔŨmin, con.A_ΔŨmax, con.A_Ymin, con.A_Ymax, con.A_x̂min, con.A_x̂max
+        con.i_b[:], con.i_C[:], con.A[:] = init_matconstraint(model,
+            i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, 
+            i_Ymin, i_Ymax, i_x̂min, i_x̂max,
+            con.A_Umin, con.A_Umax, con.A_ΔŨmin, con.A_ΔŨmax, 
+            con.A_Ymin, con.A_Ymax, con.A_x̂min, con.A_x̂max
         )
         A = con.A[con.i_b, :]
         b = con.b[con.i_b]
@@ -318,8 +321,13 @@ function setconstraint!(
         @constraint(mpc.optim, linconstraint, A*ΔŨvar .≤ b)
         setnonlincon!(mpc, model)
     else
-        i_b, _ = init_linconstraint(model, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max)
-        i_b == con.i_b || error("Cannot modify ±Inf constraints after calling moveinput!")
+        i_b, i_C = init_matconstraint(model, 
+            i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, 
+            i_Ymin, i_Ymax, i_x̂min, i_x̂max
+        )
+        if i_b ≠ con.i_b || i_C ≠ con.i_C
+            error("Cannot modify ±Inf constraints after calling moveinput!")
+        end
     end
     return mpc
 end
@@ -998,7 +1006,7 @@ function init_defaultcon(estim, Hp, Hc, C, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, k
     i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax)
     i_Ymin,  i_Ymax  = .!isinf.(Ymin),  .!isinf.(Ymax)
     i_x̂min,  i_x̂max  = .!isinf.(x̂min),  .!isinf.(x̂max)
-    i_b, A = init_linconstraint(
+    i_b, i_C, A = init_matconstraint(
         model, 
         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
@@ -1008,7 +1016,7 @@ function init_defaultcon(estim, Hp, Hc, C, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, k
         ẽx̂      , fx̂    , gx̂     , jx̂       , kx̂     , vx̂     ,  
         Umin    , Umax  , ΔŨmin  , ΔŨmax    , Ymin   , Ymax   , x̂min   , x̂max,
         A_Umin  , A_Umax, A_ΔŨmin, A_ΔŨmax  , A_Ymin , A_Ymax , A_x̂min , A_x̂max,
-        A       , b     , i_b    , c_Ymin   , c_Ymax , c_x̂min , c_x̂max ,
+        A       , b     , i_b    , c_Ymin   , c_Ymax , c_x̂min , c_x̂max , i_C
     )
     return con, S̃, Ñ_Hc, Ẽ
 end
@@ -1214,42 +1222,52 @@ init_stochpred(estim::StateEstimator, _ ) = zeros(0, estim.nxs), zeros(0, estim.
 
 
 @doc raw"""
-    init_linconstraint(::LinModel,
-        i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, args...
-    ) -> i_b, A
+    init_matconstraint(model::LinModel,
+        i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, args...
+    ) -> i_b, i_C, A
 
-Init `i_b` and `A` for the linear inequality constraints (``\mathbf{A ΔŨ ≤ b}``).
+Init `i_b`, `i_C` and `A` matrices for the linear and nonlinear inequality constraints.
 
-If provided, the arguments in `args` should be all the inequality constraint matrices:
-`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax`. If not provided, it returns an empty `A`
-matrix. `i_b` is a `BitVector` including the indices of ``\mathbf{b}`` that are finite
-numbers.
+The linear and nonlinear inequality constraints are respectively defined as:
+```math
+\begin{aligned} 
+    \mathbf{A ΔŨ } &≤ \mathbf{b} \\ 
+    \mathbf{C(ΔŨ)} &≤ \mathbf{0}
+\end{aligned}
+```
+`i_b` is a `BitVector` including the indices of ``\mathbf{b}`` that are finite numbers. 
+`i_C` is a similar vector but for the indices of ``\mathbf{C}`` (empty if `model` is a 
+[`LinModel`](@ref)). The method also returns the ``\mathbf{A}`` matrix if `args` is
+provided. In such a case, `args`  needs to contain all the inequality constraint matrices: 
+`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max`.
 """
-function init_linconstraint(::LinModel, 
+function init_matconstraint(::LinModel, 
     i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, args...
 )
     i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Ymin; i_Ymax; i_x̂min; i_x̂max]
+    i_C = BitVector()
     if isempty(args)
         A = zeros(length(i_b), 0)
     else
         A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max = args
         A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ymin; A_Ymax; A_x̂min; A_x̂max]
     end
-    return i_b, A
+    return i_b, i_C, A
 end
 
-"Init values without predicted output and terminal constraints if `model` is not a [`LinModel`](@ref)."
-function init_linconstraint(::SimModel,
-    i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, _ , _ , _ , _ , args...
+"Init `i_b` and `A` without predicted output and terminal constraints if `model` is not a [`LinModel`](@ref)."
+function init_matconstraint(::SimModel,
+    i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, args...
 )
     i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax]
+    i_C = [i_Ymin; i_Ymax; i_x̂min; i_x̂max]
     if isempty(args)
         A = zeros(length(i_b), 0)
     else
         A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , _ , _ = args
         A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax]
     end
-    return i_b, A
+    return i_b, i_C, A
 end
 
 "Validate predictive controller weight and horizon specified values."