From 9bd2b44acfbacc6ea61b7180321efa1bd73f8083 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Jan 2026 17:37:11 -0500 Subject: [PATCH 01/43] doc: API for custom linear ineq. constraints --- src/controller/construct.jl | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index d8857bf7b..431eb38ba 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -187,13 +187,24 @@ The predictive controllers support both soft and hard constraints, defined by: \mathbf{u_{min} - c_{u_{min}}} ϵ ≤&&\ \mathbf{u}(k+j) &≤ \mathbf{u_{max} + c_{u_{max}}} ϵ &&\qquad j = 0, 1 ,..., H_p - 1 \\ \mathbf{Δu_{min} - c_{Δu_{min}}} ϵ ≤&&\ \mathbf{Δu}(k+j) &≤ \mathbf{Δu_{max} + c_{Δu_{max}}} ϵ &&\qquad j = 0, 1 ,..., H_c - 1 \\ \mathbf{y_{min} - c_{y_{min}}} ϵ ≤&&\ \mathbf{ŷ}(k+j) &≤ \mathbf{y_{max} + c_{y_{max}}} ϵ &&\qquad j = 1, 2 ,..., H_p \\ + g_{min} - c_{g_{min}} ϵ ≤&&\ g(k+j) &≤ g_{max} + c_{g_{max}} ϵ &&\qquad j = 0, 1 ,..., H_p \\ \mathbf{x̂_{min} - c_{x̂_{min}}} ϵ ≤&&\ \mathbf{x̂}_i(k+j) &≤ \mathbf{x̂_{max} + c_{x̂_{max}}} ϵ &&\qquad j = H_p \end{alignat*} ``` and also ``ϵ ≥ 0``. The last line is the terminal constraints applied on the states at the end of the horizon (see Extended Help). See [`MovingHorizonEstimator`](@ref) constraints -for details on bounds and softness parameters ``\mathbf{c}``. The output and terminal -constraints are all soft by default. See Extended Help for time-varying constraints. +for details on bounds and softness parameters ``\mathbf{c}``. The custom linear constraints +are defined as: +```math + g(k+j) = + \mathbf{a_{\hat{y}}'} \mathbf{\hat{y}}(k+j) + + \mathbf{a_{r_y}'} \mathbf{\hat{r}_y}(k+j) + + \mathbf{a_{u}'} \mathbf{u}(k+j) + + \mathbf{a_{\Delta u}'} \mathbf{\Delta u}(k+j) + + \mathbf{a_{d}'} \mathbf{\hat{d}}(k+j) +``` +The output, terminal and custom linear constraints are all soft by default. See Extended +Help for time-varying constraints. # Arguments !!! info From d3cd914777a32e8bdf29fa79773ac4c8004c2c70 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Jan 2026 17:38:39 -0500 Subject: [PATCH 02/43] doc: idem --- src/controller/construct.jl | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 431eb38ba..5c29ca2fb 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -193,15 +193,15 @@ The predictive controllers support both soft and hard constraints, defined by: ``` and also ``ϵ ≥ 0``. The last line is the terminal constraints applied on the states at the end of the horizon (see Extended Help). See [`MovingHorizonEstimator`](@ref) constraints -for details on bounds and softness parameters ``\mathbf{c}``. The custom linear constraints -are defined as: +for details on bounds and softness parameters ``\mathbf{c}``. The custom linear inequality +constraints are defined as: ```math g(k+j) = - \mathbf{a_{\hat{y}}'} \mathbf{\hat{y}}(k+j) + - \mathbf{a_{r_y}'} \mathbf{\hat{r}_y}(k+j) + - \mathbf{a_{u}'} \mathbf{u}(k+j) + - \mathbf{a_{\Delta u}'} \mathbf{\Delta u}(k+j) + - \mathbf{a_{d}'} \mathbf{\hat{d}}(k+j) + \mathbf{g_{\hat{y}}'} \mathbf{\hat{y}}(k+j) + + \mathbf{g_{r_y}'} \mathbf{\hat{r}_y}(k+j) + + \mathbf{g_{u}'} \mathbf{u}(k+j) + + \mathbf{g_{\Delta u}'} \mathbf{\Delta u}(k+j) + + \mathbf{g_{d}'} \mathbf{\hat{d}}(k+j) ``` The output, terminal and custom linear constraints are all soft by default. See Extended Help for time-varying constraints. From e0052c655687d55e9f132f921c0082c543fe0d78 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 29 Jan 2026 00:56:38 -0500 Subject: [PATCH 03/43] doc: idem --- src/controller/construct.jl | 38 ++++++++++++++++++++++--------------- 1 file changed, 23 insertions(+), 15 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 5c29ca2fb..564b4da27 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -187,24 +187,14 @@ The predictive controllers support both soft and hard constraints, defined by: \mathbf{u_{min} - c_{u_{min}}} ϵ ≤&&\ \mathbf{u}(k+j) &≤ \mathbf{u_{max} + c_{u_{max}}} ϵ &&\qquad j = 0, 1 ,..., H_p - 1 \\ \mathbf{Δu_{min} - c_{Δu_{min}}} ϵ ≤&&\ \mathbf{Δu}(k+j) &≤ \mathbf{Δu_{max} + c_{Δu_{max}}} ϵ &&\qquad j = 0, 1 ,..., H_c - 1 \\ \mathbf{y_{min} - c_{y_{min}}} ϵ ≤&&\ \mathbf{ŷ}(k+j) &≤ \mathbf{y_{max} + c_{y_{max}}} ϵ &&\qquad j = 1, 2 ,..., H_p \\ - g_{min} - c_{g_{min}} ϵ ≤&&\ g(k+j) &≤ g_{max} + c_{g_{max}} ϵ &&\qquad j = 0, 1 ,..., H_p \\ \mathbf{x̂_{min} - c_{x̂_{min}}} ϵ ≤&&\ \mathbf{x̂}_i(k+j) &≤ \mathbf{x̂_{max} + c_{x̂_{max}}} ϵ &&\qquad j = H_p \end{alignat*} ``` and also ``ϵ ≥ 0``. The last line is the terminal constraints applied on the states at the end of the horizon (see Extended Help). See [`MovingHorizonEstimator`](@ref) constraints -for details on bounds and softness parameters ``\mathbf{c}``. The custom linear inequality -constraints are defined as: -```math - g(k+j) = - \mathbf{g_{\hat{y}}'} \mathbf{\hat{y}}(k+j) + - \mathbf{g_{r_y}'} \mathbf{\hat{r}_y}(k+j) + - \mathbf{g_{u}'} \mathbf{u}(k+j) + - \mathbf{g_{\Delta u}'} \mathbf{\Delta u}(k+j) + - \mathbf{g_{d}'} \mathbf{\hat{d}}(k+j) -``` -The output, terminal and custom linear constraints are all soft by default. See Extended -Help for time-varying constraints. +for details on bounds and softness parameters ``\mathbf{c}``. The output and terminal +constraints are all soft by default. See Extended Help for time-varying constraints and +custom linear constraints. # Arguments !!! info @@ -226,6 +216,8 @@ Help for time-varying constraints. - `c_x̂min=fill(1.0,nx̂)` / `c_x̂max=fill(1.0,nx̂)` : `x̂min` / `x̂max` softness weight ``\mathbf{c_{x̂_{min/max}}}`` - all the keyword arguments above but with a first capital letter, except for the terminal constraints, e.g. `Ymax` or `C_Δumin`: for time-varying constraints (see Extended Help) +- `Gmin` / `Gmax` / `C_Gmin` / `C_Gmax` : custom linear bounds and softness weights over the + prediction horizon, default bounds are `±Inf` and weights are `1.0` (see Extended Help) # Examples ```jldoctest @@ -268,18 +260,34 @@ LinMPC controller with a sample time Ts = 4.0 s: create the controller (but different than `±Inf`). It is also possible to specify time-varying constraints over ``H_p`` and ``H_c`` - horizons. In such a case, they are defined by: + horizons. The custom linear inequality constraints are also time-varying but over + ``H_p + 1`` steps. In such cases, they are all defined by: ```math \begin{alignat*}{3} \mathbf{U_{min} - C_{u_{min}}} ϵ ≤&&\ \mathbf{U} &≤ \mathbf{U_{max} + C_{u_{max}}} ϵ \\ \mathbf{ΔU_{min} - C_{Δu_{min}}} ϵ ≤&&\ \mathbf{ΔU} &≤ \mathbf{ΔU_{max} + C_{Δu_{max}}} ϵ \\ - \mathbf{Y_{min} - C_{y_{min}}} ϵ ≤&&\ \mathbf{Ŷ} &≤ \mathbf{Y_{max} + C_{y_{max}}} ϵ + \mathbf{Y_{min} - C_{y_{min}}} ϵ ≤&&\ \mathbf{Ŷ} &≤ \mathbf{Y_{max} + C_{y_{max}}} ϵ \\ + \mathbf{G_{min} - C_{G_{min}}} ϵ ≤&&\ \mathbf{G} &≤ \mathbf{G_{max} + C_{G_{max}}} ϵ \end{alignat*} ``` For this, use the same keyword arguments as above but with a first capital letter: + - `Umin` / `Umax` / `C_umin` / `C_umax` : ``\mathbf{U}`` constraints `(nu*Hp,)`. - `ΔUmin` / `ΔUmax` / `C_Δumin` / `C_Δumax` : ``\mathbf{ΔU}`` constraints `(nu*Hc,)`. - `Ymin` / `Ymax` / `C_ymin` / `C_ymax` : ``\mathbf{Ŷ}`` constraints `(ny*Hp,)`. + - `Gmin` / `Gmax` / `C_Gmin` / `C_Gmax` : custom linear constraints `(nG*(Hp+1),)`. + + The custom constraints are all gathered in the vector: + ```math + \mathbf{G} = \begin{bmatrix} + \mathbf{G_x̂ x̂}_i(k+0) + \mathbf{G_u u}(k+0) + \mathbf{G_d d}(k+0) + \mathbf{G_r r_y}(k+0) \\ + \mathbf{G_x̂ x̂}_i(k+1) + \mathbf{G_u u}(k+1) + \mathbf{G_d d̂}(k+1) + \mathbf{G_r r̂_y}(k+1) \\ + \vdots \\ + \mathbf{G_x̂ x̂}_i(k+H_p) + \mathbf{G_u u}(k+H_p) + \mathbf{G_d d̂}(k+H_p) + \mathbf{G_r r̂_y}(k+H_p) \end{bmatrix} + ``` + The matrices ``\mathbf{G_x̂}``, ``\mathbf{G_u}``, ``\mathbf{G_d}`` and ``\mathbf{G_r}`` + have `nG` rows and are provided at construction time. The terms with ``\mathbf{G_x̂}`` + are present only if the model is a [`LinModel`](@ref). """ function setconstraint!( mpc::PredictiveController; From d0522891fcccf92f36ded7312ae8b82c22a8e7f5 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 29 Jan 2026 11:35:15 -0500 Subject: [PATCH 04/43] doc: also supported by `MultipleShooting` --- src/controller/construct.jl | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 564b4da27..61d5e8abf 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -287,7 +287,8 @@ LinMPC controller with a sample time Ts = 4.0 s: ``` The matrices ``\mathbf{G_x̂}``, ``\mathbf{G_u}``, ``\mathbf{G_d}`` and ``\mathbf{G_r}`` have `nG` rows and are provided at construction time. The terms with ``\mathbf{G_x̂}`` - are present only if the model is a [`LinModel`](@ref). + are present only if the model is a [`LinModel`](@ref) or if the transcription is a + [`MultipleShooting`](@ref). """ function setconstraint!( mpc::PredictiveController; From 16b0e9ca4f8e4183095dbefc763c4249335f7c5f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 29 Jan 2026 15:22:16 -0500 Subject: [PATCH 05/43] doc: more precise applicability --- src/controller/construct.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 61d5e8abf..b820b20f1 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -287,8 +287,8 @@ LinMPC controller with a sample time Ts = 4.0 s: ``` The matrices ``\mathbf{G_x̂}``, ``\mathbf{G_u}``, ``\mathbf{G_d}`` and ``\mathbf{G_r}`` have `nG` rows and are provided at construction time. The terms with ``\mathbf{G_x̂}`` - are present only if the model is a [`LinModel`](@ref) or if the transcription is a - [`MultipleShooting`](@ref). + are present only if the model is a [`LinModel`](@ref) or if the transcription is not a + [`SingleShooting`](@ref). """ function setconstraint!( mpc::PredictiveController; From e094a6d52394e7204465073901561c8b6720c8c5 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 29 Jan 2026 20:04:51 -0500 Subject: [PATCH 06/43] doc: output instead of state in custom linear constraints --- src/controller/construct.jl | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index b820b20f1..ce088729e 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -279,11 +279,11 @@ LinMPC controller with a sample time Ts = 4.0 s: The custom constraints are all gathered in the vector: ```math - \mathbf{G} = \begin{bmatrix} - \mathbf{G_x̂ x̂}_i(k+0) + \mathbf{G_u u}(k+0) + \mathbf{G_d d}(k+0) + \mathbf{G_r r_y}(k+0) \\ - \mathbf{G_x̂ x̂}_i(k+1) + \mathbf{G_u u}(k+1) + \mathbf{G_d d̂}(k+1) + \mathbf{G_r r̂_y}(k+1) \\ - \vdots \\ - \mathbf{G_x̂ x̂}_i(k+H_p) + \mathbf{G_u u}(k+H_p) + \mathbf{G_d d̂}(k+H_p) + \mathbf{G_r r̂_y}(k+H_p) \end{bmatrix} + \mathbf{G} = \begin{bmatrix} + \mathbf{G_ŷ ŷ}(k+0) + \mathbf{G_u u}(k+0) + \mathbf{G_d d}(k+0) + \mathbf{G_r r_y}(k+0) \\ + \mathbf{G_ŷ ŷ}(k+1) + \mathbf{G_u u}(k+1) + \mathbf{G_d d̂}(k+1) + \mathbf{G_r r̂_y}(k+1) \\ + \vdots \\ + \mathbf{G_ŷ ŷ}(k+H_p) + \mathbf{G_u u}(k+H_p) + \mathbf{G_d d̂}(k+H_p) + \mathbf{G_r r̂_y}(k+H_p) \end{bmatrix} ``` The matrices ``\mathbf{G_x̂}``, ``\mathbf{G_u}``, ``\mathbf{G_d}`` and ``\mathbf{G_r}`` have `nG` rows and are provided at construction time. The terms with ``\mathbf{G_x̂}`` From b1038bfec122efa439175cc979cd1d791e75a7ce Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 29 Jan 2026 20:50:39 -0500 Subject: [PATCH 07/43] changed: output instead of state in custom lin. constraint --- src/controller/construct.jl | 30 +++++++++++++++++++++++------- src/controller/linmpc.jl | 20 ++++++++++++++++++-- src/controller/transcription.jl | 22 ++++++++++++++++++++-- 3 files changed, 61 insertions(+), 11 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index ce088729e..69d57db2b 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -280,15 +280,14 @@ LinMPC controller with a sample time Ts = 4.0 s: The custom constraints are all gathered in the vector: ```math \mathbf{G} = \begin{bmatrix} - \mathbf{G_ŷ ŷ}(k+0) + \mathbf{G_u u}(k+0) + \mathbf{G_d d}(k+0) + \mathbf{G_r r_y}(k+0) \\ - \mathbf{G_ŷ ŷ}(k+1) + \mathbf{G_u u}(k+1) + \mathbf{G_d d̂}(k+1) + \mathbf{G_r r̂_y}(k+1) \\ + \mathbf{G_y ŷ}(k+0) + \mathbf{G_u u}(k+0) + \mathbf{G_d d}(k+0) + \mathbf{G_r r_y}(k+0) \\ + \mathbf{G_y ŷ}(k+1) + \mathbf{G_u u}(k+1) + \mathbf{G_d d̂}(k+1) + \mathbf{G_r r̂_y}(k+1) \\ \vdots \\ - \mathbf{G_ŷ ŷ}(k+H_p) + \mathbf{G_u u}(k+H_p) + \mathbf{G_d d̂}(k+H_p) + \mathbf{G_r r̂_y}(k+H_p) \end{bmatrix} + \mathbf{G_y ŷ}(k+H_p) + \mathbf{G_u u}(k+H_p) + \mathbf{G_d d̂}(k+H_p) + \mathbf{G_r r̂_y}(k+H_p) \end{bmatrix} ``` - The matrices ``\mathbf{G_x̂}``, ``\mathbf{G_u}``, ``\mathbf{G_d}`` and ``\mathbf{G_r}`` - have `nG` rows and are provided at construction time. The terms with ``\mathbf{G_x̂}`` - are present only if the model is a [`LinModel`](@ref) or if the transcription is not a - [`SingleShooting`](@ref). + The matrices ``\mathbf{G_y}``, ``\mathbf{G_u}``, ``\mathbf{G_d}`` and ``\mathbf{G_r}`` + have `nG` rows and are provided at construction time. The terms with ``\mathbf{G_y}`` + are present only if the model is a [`LinModel`](@ref). """ function setconstraint!( mpc::PredictiveController; @@ -576,6 +575,23 @@ end "Get the actual control Horizon `Hc` (integer) from the move blocking vector `nb`." get_Hc(nb::AbstractVector{Int}) = length(nb) +"Validate the custom linear constraint matrices dimensions." +function validate_custom_lincon(model::SimModel, Gy, Gu, Gd, Gr) + nu, nd, ny = model.nu, model.nd, model.ny + isnothing(Gy) && (Gy = zeros(NT, 0, nx̂)) + isnothing(Gu) && (Gu = zeros(NT, 0, nu)) + isnothing(Gd) && (Gd = zeros(NT, 0, nd)) + isnothing(Gr) && (Gr = zeros(NT, 0, ny)) + size(Gy, 2) == ny || throw(DimensionMismatch("Gy must have $ny columns.")) + size(Gu, 2) == nu || throw(DimensionMismatch("Gu must have $nu columns.")) + size(Gd, 2) == nd || throw(DimensionMismatch("Gd must have $nd columns.")) + size(Gr, 2) == ny || throw(DimensionMismatch("Gr must have $ny columns.")) + nG = size(Gy, 1) + size(Gu, 1) == nG || throw(DimensionMismatch("Gu must have $nG rows.")) + size(Gd, 1) == nG || throw(DimensionMismatch("Gd must have $nG rows.")) + size(Gr, 1) == nG || throw(DimensionMismatch("Gr must have $nG rows.")) + return nothing +end """ validate_args(mpc::PredictiveController, ry, d, lastu, D̂, R̂y, R̂u) diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index d7d30410d..735c5a985 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -147,6 +147,7 @@ This method uses the default state estimator, a [`SteadyKalmanFilter`](@ref) wit arguments. This controller allocates memory at each time step for the optimization. # Arguments + - `model::LinModel` : model used for controller predictions and state estimations. - `Hp::Int=10+nk` : prediction horizon ``H_p``, `nk` is the number of delays in `model`. - `Hc::Union{Int, Vector{Int}}=2` : control horizon ``H_c``, custom move blocking pattern is @@ -158,6 +159,10 @@ arguments. This controller allocates memory at each time step for the optimizati - `N_Hc=Diagonal(repeat(Nwt,Hc))` : positive semidefinite symmetric matrix ``\mathbf{N}_{H_c}``. - `L_Hp=Diagonal(repeat(Lwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{L}_{H_p}``. - `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only. +- `Gy=zeros(0,model.ny)` : custom linear constraint matrix for output (see Extended Help). +- `Gu=zeros(0,model.nu)` : custom linear constraint matrix for manipulated input (see Extended Help). +- `Gd=zeros(0,model.nd)` : custom linear constraint matrix for meas. disturbance (see Extended Help). +- `Gr=zeros(0,model.ny)` : custom linear constraint matrix for output setpoint (see Extended Help). - `transcription=SingleShooting()` : [`SingleShooting`](@ref) or [`MultipleShooting`](@ref). - `optim=JuMP.Model(OSQP.MathOptInterfaceOSQP.Optimizer)` : quadratic optimizer used in the predictive controller, provided as a [`JuMP.Model`](@extref) object (default to @@ -222,12 +227,19 @@ function LinMPC( N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))), L_Hp = Diagonal(repeat(Lwt, Hp)), Cwt = DEFAULT_CWT, + Gy = zeros(0, model.ny), + Gu = zeros(0, model.nu), + Gd = zeros(0, model.nd), + Gr = zeros(0, model.ny), transcription::ShootingMethod = DEFAULT_LINMPC_TRANSCRIPTION, optim::JuMP.GenericModel = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false), kwargs... ) estim = SteadyKalmanFilter(model; kwargs...) - return LinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, M_Hp, N_Hc, L_Hp, transcription, optim) + return LinMPC( + estim; + Hp, Hc, Mwt, Nwt, Lwt, Cwt, M_Hp, N_Hc, L_Hp, Gy, Gu, Gd, Gr, transcription, optim + ) end @@ -271,8 +283,12 @@ function LinMPC( N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))), L_Hp = Diagonal(repeat(Lwt, Hp)), Cwt = DEFAULT_CWT, + Gy = zeros(0, estim.model.ny), + Gu = zeros(0, estim.model.nu), + Gd = zeros(0, estim.model.nd), + Gr = zeros(0, estim.model.ny), transcription::ShootingMethod = DEFAULT_LINMPC_TRANSCRIPTION, - optim::JM = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false), + optim::JM = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false) ) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel} isa(estim.model, LinModel) || error(MSG_LINMODEL_ERR) nk = estimate_delays(estim.model) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index a7b503bea..72fa66451 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -134,6 +134,25 @@ function get_nZ(estim::StateEstimator, ::TranscriptionMethod, Hp, Hc) return estim.model.nu*Hc + estim.nx̂*Hp end +function custom_lincon( + model::LinModel, ::TranscriptionMethod, Gy, Gu, Gd, Gr +) where {NT<:Real} + validate_custom_lincon(model, Gy, Gu, Gd, Gr) + +end + +function custom_lincon( + model::NonLinModel, ::SingleShooting, Gy, Gu, Gd, Gr +) + validate_custom_lincon(model, Gy, Gu, Gd, Gr) +end + +function custom_lincon( + model::NonLinModel, ::TranscriptionMethod, Gy, Gu, Gd, Gr +) + validate_custom_lincon(model, Gy, Gu, Gd, Gr) +end + "Get length of the `k` vector with all the solver intermediate steps or all the collocation pts." get_nk(model::SimModel, ::ShootingMethod) = model.nk get_nk(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.nc @@ -156,8 +175,7 @@ in which ``\mathbf{P_{Δu}}`` is defined in the Extended Help section. Following the decision variable definition of the [`TranscriptionMethod`](@ref), the conversion matrix ``\mathbf{P_{Δu}}``, we have: - ``\mathbf{P_{Δu}} = \mathbf{I}`` if `transcription` is a [`SingleShooting`](@ref) - - ``\mathbf{P_{Δu}} = [\begin{smallmatrix}\mathbf{I} & \mathbf{0} \end{smallmatrix}]`` - if `transcription` is a [`MultipleShooting`](@ref) + - ``\mathbf{P_{Δu}} = [\begin{smallmatrix}\mathbf{I} & \mathbf{0} \end{smallmatrix}]`` otherwise. The matrix is store as as `SparseMatrixCSC` to support both cases efficiently. """ function init_ZtoΔU end From 3819fada03d7210005f292aae48a70def3738464 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 29 Jan 2026 21:36:32 -0500 Subject: [PATCH 08/43] wip: custom linear constraints --- src/controller/construct.jl | 34 +++++++++++++------ src/controller/linmpc.jl | 8 +++-- src/controller/nonlinmpc.jl | 2 ++ src/controller/transcription.jl | 60 +++++++++++++++++++++------------ 4 files changed, 70 insertions(+), 34 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 69d57db2b..e65ad0925 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -143,6 +143,8 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} Y0max ::Vector{NT} x̂0min ::Vector{NT} x̂0max ::Vector{NT} + Gmin ::Vector{NT} + Gmax ::Vector{NT} # A matrices for the linear inequality constraints: A_Umin ::SparseMatrixCSC{NT, Int} A_Umax ::SparseMatrixCSC{NT, Int} @@ -152,6 +154,8 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} A_Ymax ::Matrix{NT} A_x̂min ::Matrix{NT} A_x̂max ::Matrix{NT} + A_Gmin ::Matrix{NT} + A_Gmax ::Matrix{NT} A ::Matrix{NT} # b vector for the linear inequality constraints: b ::Vector{NT} @@ -164,6 +168,8 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} beq ::Vector{NT} # nonlinear equality constraints: neq ::Int + # custom linear equality constraints: + nG ::Int # constraint softness parameter vectors for the nonlinear inequality constraints: C_ymin ::Vector{NT} C_ymax ::Vector{NT} @@ -286,8 +292,8 @@ LinMPC controller with a sample time Ts = 4.0 s: \mathbf{G_y ŷ}(k+H_p) + \mathbf{G_u u}(k+H_p) + \mathbf{G_d d̂}(k+H_p) + \mathbf{G_r r̂_y}(k+H_p) \end{bmatrix} ``` The matrices ``\mathbf{G_y}``, ``\mathbf{G_u}``, ``\mathbf{G_d}`` and ``\mathbf{G_r}`` - have `nG` rows and are provided at construction time. The terms with ``\mathbf{G_y}`` - are present only if the model is a [`LinModel`](@ref). + are provided at construction time and they must have the same number of rows. The terms + with ``\mathbf{G_y}`` are present only if the model is a [`LinModel`](@ref). """ function setconstraint!( mpc::PredictiveController; @@ -586,10 +592,10 @@ function validate_custom_lincon(model::SimModel, Gy, Gu, Gd, Gr) size(Gu, 2) == nu || throw(DimensionMismatch("Gu must have $nu columns.")) size(Gd, 2) == nd || throw(DimensionMismatch("Gd must have $nd columns.")) size(Gr, 2) == ny || throw(DimensionMismatch("Gr must have $ny columns.")) - nG = size(Gy, 1) - size(Gu, 1) == nG || throw(DimensionMismatch("Gu must have $nG rows.")) - size(Gd, 1) == nG || throw(DimensionMismatch("Gd must have $nG rows.")) - size(Gr, 1) == nG || throw(DimensionMismatch("Gr must have $nG rows.")) + nGr = size(Gr, 1) + size(Gy, 1) == nGr || throw(DimensionMismatch("Gy must have $nGr rows.")) + size(Gu, 1) == nGr || throw(DimensionMismatch("Gu must have $nGr rows.")) + size(Gd, 1) == nGr || throw(DimensionMismatch("Gd must have $nGr rows.")) return nothing end @@ -676,6 +682,7 @@ verify_cond(::TranscriptionMethod,_,_) = nothing PΔu, Pu, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + Gy, Gu, Gd, Gr, gc!=nothing, nc=0 ) -> con, nϵ, P̃Δu, P̃u, Ẽ @@ -691,19 +698,23 @@ function init_defaultcon_mpc( PΔu, Pu, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + Gy, Gu, Gd, Gr, gc!::GCfunc = nothing, nc = 0 ) where {NT<:Real, GCfunc<:Union{Nothing, Function}} model = estim.model nu, ny, nx̂ = model.nu, model.ny, estim.nx̂ + nG = size(Gr, 1)*(Hp+1) nϵ = weights.isinf_C ? 0 : 1 u0min, u0max = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu) Δumin, Δumax = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu) y0min, y0max = fill(convert(NT,-Inf), ny), fill(convert(NT,+Inf), ny) x̂0min, x̂0max = fill(convert(NT,-Inf), nx̂), fill(convert(NT,+Inf), nx̂) + Gmin, Gmax = fill(convert(NT,-Inf), nG), fill(convert(NT,+Inf), nG) c_umin, c_umax = fill(zero(NT), nu), fill(zero(NT), nu) c_Δumin, c_Δumax = fill(zero(NT), nu), fill(zero(NT), nu) c_ymin, c_ymax = fill(one(NT), ny), fill(one(NT), ny) c_x̂min, c_x̂max = fill(one(NT), nx̂), fill(one(NT), nx̂) + C_Gmin, C_Gmax = fill(one(NT), nG), fill(one(NT), nG) U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max = repeat_constraints(Hp, Hc, u0min, u0max, Δumin, Δumax, y0min, y0max) C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax = @@ -712,15 +723,17 @@ function init_defaultcon_mpc( A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃Δu = relaxΔU(PΔu, C_Δumin, C_Δumax, ΔUmin, ΔUmax, nϵ) A_Ymin, A_Ymax, Ẽ = relaxŶ(E, C_ymin, C_ymax, nϵ) A_x̂min, A_x̂max, ẽx̂ = relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ) + A_Gmin, A_Gmax = custom_lincon(mode, transcription, nG, Gy, Gu, Gd, Gr, Ẽ) A_ŝ, Ẽŝ = augmentdefect(Eŝ, nϵ) i_Umin, i_Umax = .!isinf.(U0min), .!isinf.(U0max) 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_Gmin, i_Gmax = .!isinf.(Gmin), .!isinf.(Gmax) 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, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, i_Gmin, i_Gmax, + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂max, A_x̂min, A_Gmin, A_Gmax, A_ŝ ) # dummy b and beq vectors (updated just before optimization) @@ -728,12 +741,13 @@ function init_defaultcon_mpc( con = ControllerConstraint{NT, GCfunc}( ẽx̂ , fx̂ , gx̂ , jx̂ , kx̂ , vx̂ , bx̂ , Ẽŝ , Fŝ , Gŝ , Jŝ , Kŝ , Vŝ , Bŝ , - U0min , U0max , ΔŨmin , ΔŨmax , Y0min , Y0max , x̂0min , x̂0max, - A_Umin , A_Umax , A_ΔŨmin, A_ΔŨmax , A_Ymin , A_Ymax , A_x̂min , A_x̂max, + U0min , U0max , ΔŨmin , ΔŨmax , Y0min , Y0max , x̂0min , x̂0max , Gmin , Gmax, + A_Umin , A_Umax , A_ΔŨmin, A_ΔŨmax , A_Ymin , A_Ymax , A_x̂min , A_x̂max , A_Gmin , A_Gmax, A , b , i_b , A_ŝ , Aeq , beq , neq , + nG, C_ymin , C_ymax , c_x̂min , c_x̂max , i_g, gc! , nc ) diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 735c5a985..344999860 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -48,7 +48,8 @@ struct LinMPC{ Dop::Vector{NT} buffer::PredictiveControllerBuffer{NT} function LinMPC{NT}( - estim::SE, Hp, Hc, nb, weights::CW, + estim::SE, Hp, Hc, nb, weights::CW, + Gy, Gu, Gd, Gr, transcription::TM, optim::JM ) where { NT<:Real, @@ -77,7 +78,8 @@ struct LinMPC{ Hp, Hc, PΔu, Pu, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ + Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + Gy, Gu, Gd, Gr ) H̃ = init_quadprog(model, transcription, weights, Ẽ, P̃Δu, P̃u) # dummy vals (updated just before optimization): @@ -299,7 +301,7 @@ function LinMPC( nb = move_blocking(Hp, Hc) Hc = get_Hc(nb) weights = ControllerWeights(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt) - return LinMPC{NT}(estim, Hp, Hc, nb, weights, transcription, optim) + return LinMPC{NT}(estim, Hp, Hc, nb, weights, Gy, Gu, Gd, Gr, transcription, optim) end """ diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 053a1095d..9a5edb6dd 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -100,12 +100,14 @@ struct NonLinMPC{ Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) # dummy vals (updated just before optimization): F, fx̂, Fŝ = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp) + Gy, Gu, Gd, Gr = zeros(0, ny), zeros(0, nu), zeros(0, nd), zeros(0, ny) # TODO: DELETE THIS !!!! con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc( estim, weights, transcription, Hp, Hc, PΔu, Pu, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + Gy, Gu, Gd, Gr, gc!, nc ) warn_cond = iszero(weights.E) ? 1e6 : Inf # condition number warning only if Ewt==0 diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 72fa66451..6dd58e0ce 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -135,22 +135,27 @@ function get_nZ(estim::StateEstimator, ::TranscriptionMethod, Hp, Hc) end function custom_lincon( - model::LinModel, ::TranscriptionMethod, Gy, Gu, Gd, Gr + model::LinModel{NT}, ::TranscriptionMethod, nG, Gy, Gu, Gd, Gr, Ẽ ) where {NT<:Real} validate_custom_lincon(model, Gy, Gu, Gd, Gr) - + A_Gmin, A_Gmax = zeros(NT, nG, size(Ẽ,2)), zeros(NT, nG, size(Ẽ,2)) + return A_Gmin, A_Gmax end function custom_lincon( - model::NonLinModel, ::SingleShooting, Gy, Gu, Gd, Gr -) + model::NonLinModel{NT}, ::SingleShooting, nG, Gy, Gu, Gd, Gr, Ẽ +) where {NT<:Real} validate_custom_lincon(model, Gy, Gu, Gd, Gr) + A_Gmin, A_Gmax = zeros(NT, nG, size(Ẽ,2)), zeros(NT, nG, size(Ẽ,2)) + return A_Gmin, A_Gmax end function custom_lincon( - model::NonLinModel, ::TranscriptionMethod, Gy, Gu, Gd, Gr -) + model::NonLinModel{NT}, ::TranscriptionMethod, nG, Gy, Gu, Gd, Gr, Ẽ +) where {NT<:Real} validate_custom_lincon(model, Gy, Gu, Gd, Gr) + A_Gmin, A_Gmax = zeros(NT, nG, size(Ẽ,2)), zeros(NT, nG, size(Ẽ,2)) + return A_Gmin, A_Gmax end "Get length of the `k` vector with all the solver intermediate steps or all the collocation pts." @@ -692,7 +697,7 @@ end @doc raw""" init_matconstraint_mpc( model::LinModel, transcription::TranscriptionMethod, nc::Int, - i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, i_Gmin, i_Gmax, args... ) -> i_b, i_g, A, Aeq, neq @@ -712,23 +717,36 @@ The argument `nc` is the number of custom nonlinear inequality constraints in 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 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}}``. +`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_Gmin, A_Gmax, 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, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, i_Gmin, i_Gmax, args... ) where {NT<:Real} if isempty(args) 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] + ( + A_Umin, A_Umax, + A_ΔŨmin, A_ΔŨmax, + A_Ymin, A_Ymax, + A_x̂min, A_x̂max, + A_Gmin, A_Gmax, + A_ŝ + ) = args + A = [ + A_Umin; A_Umax; + A_ΔŨmin; A_ΔŨmax; + A_Ymin; A_Ymax; + A_x̂min; A_x̂max; + A_Gmin; A_Gmax + ] 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_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Ymin; i_Ymax; i_x̂min; i_x̂max; i_Gmin; i_Gmax] i_g = trues(nc) return i_b, i_g, A, Aeq, neq end @@ -736,18 +754,18 @@ end "Init `i_b` without output & terminal constraints if `NonLinModel` and `SingleShooting`." function init_matconstraint_mpc( ::NonLinModel{NT}, ::SingleShooting, nc::Int, - i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, i_Gmin, i_Gmax, args... ) where {NT<:Real} if isempty(args) A, Aeq, neq = nothing, nothing, nothing else - A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , _ , _ , A_ŝ = args - A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax] + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , _ , _ , A_Gmin, A_Gmax, A_ŝ = args + A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Gmin; A_Gmax] Aeq = A_ŝ neq = 0 end - i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax] + i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Gmin; i_Gmax] i_g = [i_Ymin; i_Ymax; i_x̂min; i_x̂max; trues(nc)] return i_b, i_g, A, Aeq, neq end @@ -755,19 +773,19 @@ end "Init `i_b` without output constraints if `NonLinModel` and other `TranscriptionMethod`." function init_matconstraint_mpc( ::NonLinModel{NT}, ::TranscriptionMethod, nc::Int, - i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, i_Gmin, i_Gmax, args... ) where {NT<:Real} if isempty(args) A, Aeq, neq = nothing, nothing, nothing else - A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_x̂min, A_x̂max, A_ŝ = args - A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_x̂min; A_x̂max] + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Gmin, A_Gmax, A_x̂min, A_x̂max, A_ŝ = args + A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_x̂min; A_x̂max; A_Gmin; A_Gmax] 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_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_x̂min; i_x̂max; i_Gmin; i_Gmax] i_g = [i_Ymin; i_Ymax; trues(nc)] return i_b, i_g, A, Aeq, neq end From ac4510e3d4c67a2610cc47608205641c7d7c89d1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 29 Jan 2026 21:51:34 -0500 Subject: [PATCH 09/43] debug: correct variable name and signatures --- src/controller/construct.jl | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index e65ad0925..f5cc5df7e 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -449,13 +449,16 @@ function setconstraint!( i_ΔŨmin, i_ΔŨmax = .!isinf.(con.ΔŨmin), .!isinf.(con.ΔŨmax) i_Ymin, i_Ymax = .!isinf.(con.Y0min), .!isinf.(con.Y0max) i_x̂min, i_x̂max = .!isinf.(con.x̂0min), .!isinf.(con.x̂0max) + i_Gmin, i_Gmax = .!isinf.(con.Gmin), .!isinf.(con.Gmax) if notSolvedYet con.i_b[:], con.i_g[:], con.A[:] = init_matconstraint_mpc( model, transcription, nc, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, + i_Gmin, i_Gmax, 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.A_Gmin, con.A_Gmax, con.A_ŝ ) A = con.A[con.i_b, :] @@ -469,7 +472,8 @@ function setconstraint!( i_b, i_g = init_matconstraint_mpc( model, transcription, nc, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, - i_Ymin, i_Ymax, i_x̂min, i_x̂max + i_Ymin, i_Ymax, i_x̂min, i_x̂max, + i_Gmin, i_Gmax ) if i_b ≠ con.i_b || i_g ≠ con.i_g error("Cannot modify ±Inf constraints after calling moveinput!") @@ -723,7 +727,7 @@ function init_defaultcon_mpc( A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃Δu = relaxΔU(PΔu, C_Δumin, C_Δumax, ΔUmin, ΔUmax, nϵ) A_Ymin, A_Ymax, Ẽ = relaxŶ(E, C_ymin, C_ymax, nϵ) A_x̂min, A_x̂max, ẽx̂ = relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ) - A_Gmin, A_Gmax = custom_lincon(mode, transcription, nG, Gy, Gu, Gd, Gr, Ẽ) + A_Gmin, A_Gmax = custom_lincon(model, transcription, nG, Gy, Gu, Gd, Gr, Ẽ) A_ŝ, Ẽŝ = augmentdefect(Eŝ, nϵ) i_Umin, i_Umax = .!isinf.(U0min), .!isinf.(U0max) i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax) From f9d200974e74af1e379a9ee866de8041bb1498b2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 29 Jan 2026 22:46:27 -0500 Subject: [PATCH 10/43] added: storing repeated custom linear constraint matrices --- src/controller/construct.jl | 18 ++++++++++--- src/controller/linmpc.jl | 5 ++++ src/controller/transcription.jl | 45 +++++++++++++++------------------ 3 files changed, 41 insertions(+), 27 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index f5cc5df7e..f28c33af1 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -169,6 +169,10 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} # nonlinear equality constraints: neq ::Int # custom linear equality constraints: + Ḡy ::SparseMatrixCSC{NT, Int} + Ḡu ::SparseMatrixCSC{NT, Int} + Ḡd ::SparseMatrixCSC{NT, Int} + Ḡr ::SparseMatrixCSC{NT, Int} nG ::Int # constraint softness parameter vectors for the nonlinear inequality constraints: C_ymin ::Vector{NT} @@ -707,6 +711,7 @@ function init_defaultcon_mpc( ) where {NT<:Real, GCfunc<:Union{Nothing, Function}} model = estim.model nu, ny, nx̂ = model.nu, model.ny, estim.nx̂ + validate_custom_lincon(model, Gy, Gu, Gd, Gr) nG = size(Gr, 1)*(Hp+1) nϵ = weights.isinf_C ? 0 : 1 u0min, u0max = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu) @@ -727,7 +732,11 @@ function init_defaultcon_mpc( A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃Δu = relaxΔU(PΔu, C_Δumin, C_Δumax, ΔUmin, ΔUmax, nϵ) A_Ymin, A_Ymax, Ẽ = relaxŶ(E, C_ymin, C_ymax, nϵ) A_x̂min, A_x̂max, ẽx̂ = relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ) - A_Gmin, A_Gmax = custom_lincon(model, transcription, nG, Gy, Gu, Gd, Gr, Ẽ) + Ḡy = sparse(repeatdiag(Gy, Hp+1)) + Ḡu = sparse(repeatdiag(Gu, Hp+1)) + Ḡd = sparse(repeatdiag(Gd, Hp+1)) + Ḡr = sparse(repeatdiag(Gr, Hp+1)) + A_Gmin, A_Gmax = custom_lincon(model, transcription, nG, Ḡy, Ḡu, Ḡd, Ḡr, Ẽ) A_ŝ, Ẽŝ = augmentdefect(Eŝ, nϵ) i_Umin, i_Umax = .!isinf.(U0min), .!isinf.(U0max) i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax) @@ -745,12 +754,15 @@ function init_defaultcon_mpc( con = ControllerConstraint{NT, GCfunc}( ẽx̂ , fx̂ , gx̂ , jx̂ , kx̂ , vx̂ , bx̂ , Ẽŝ , Fŝ , Gŝ , Jŝ , Kŝ , Vŝ , Bŝ , - U0min , U0max , ΔŨmin , ΔŨmax , Y0min , Y0max , x̂0min , x̂0max , Gmin , Gmax, - A_Umin , A_Umax , A_ΔŨmin, A_ΔŨmax , A_Ymin , A_Ymax , A_x̂min , A_x̂max , A_Gmin , A_Gmax, + U0min , U0max , ΔŨmin , ΔŨmax , Y0min , Y0max , x̂0min , x̂0max , + Gmin , Gmax , + A_Umin , A_Umax , A_ΔŨmin, A_ΔŨmax , A_Ymin , A_Ymax , A_x̂min , A_x̂max , + A_Gmin , A_Gmax , A , b , i_b , A_ŝ , Aeq , beq , neq , + Ḡy , Ḡu , Ḡd , Ḡr , nG, C_ymin , C_ymax , c_x̂min , c_x̂max , i_g, gc! , nc diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 344999860..1e56a41c8 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -198,6 +198,11 @@ LinMPC controller with a sample time Ts = 4.0 s: for over-actuated systems, when `nu > ny` (e.g. prioritize solutions with lower economical costs). The default `Lwt` value implies that this feature is disabled by default. + The custom linear constraint matrices `Gy`, `Gu`, `Gd`, and `Gr` allow to define + constraints based on linear combinations of outputs, manipulated inputs, measured + disturbances, and output setpoints, respectively. See the Extended Help section in + [`setconstraint!`](@ref) documentation for more details. + The objective function follows this nomenclature: | VARIABLE | DESCRIPTION | SIZE | diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 6dd58e0ce..3d0eab4e0 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -134,30 +134,6 @@ function get_nZ(estim::StateEstimator, ::TranscriptionMethod, Hp, Hc) return estim.model.nu*Hc + estim.nx̂*Hp end -function custom_lincon( - model::LinModel{NT}, ::TranscriptionMethod, nG, Gy, Gu, Gd, Gr, Ẽ -) where {NT<:Real} - validate_custom_lincon(model, Gy, Gu, Gd, Gr) - A_Gmin, A_Gmax = zeros(NT, nG, size(Ẽ,2)), zeros(NT, nG, size(Ẽ,2)) - return A_Gmin, A_Gmax -end - -function custom_lincon( - model::NonLinModel{NT}, ::SingleShooting, nG, Gy, Gu, Gd, Gr, Ẽ -) where {NT<:Real} - validate_custom_lincon(model, Gy, Gu, Gd, Gr) - A_Gmin, A_Gmax = zeros(NT, nG, size(Ẽ,2)), zeros(NT, nG, size(Ẽ,2)) - return A_Gmin, A_Gmax -end - -function custom_lincon( - model::NonLinModel{NT}, ::TranscriptionMethod, nG, Gy, Gu, Gd, Gr, Ẽ -) where {NT<:Real} - validate_custom_lincon(model, Gy, Gu, Gd, Gr) - A_Gmin, A_Gmax = zeros(NT, nG, size(Ẽ,2)), zeros(NT, nG, size(Ẽ,2)) - return A_Gmin, A_Gmax -end - "Get length of the `k` vector with all the solver intermediate steps or all the collocation pts." get_nk(model::SimModel, ::ShootingMethod) = model.nk get_nk(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.nc @@ -694,6 +670,27 @@ function init_defectmat( return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ end +function custom_lincon( + model::LinModel{NT}, ::TranscriptionMethod, nG, Ḡy, Ḡu, Ḡd, Ḡr, Ẽ +) where {NT<:Real} + A_Gmin, A_Gmax = zeros(NT, nG, size(Ẽ,2)), zeros(NT, nG, size(Ẽ,2)) + return A_Gmin, A_Gmax +end + +function custom_lincon( + model::NonLinModel{NT}, ::SingleShooting, nG, Ḡy, Ḡu, Ḡd, Ḡr, Ẽ +) where {NT<:Real} + A_Gmin, A_Gmax = zeros(NT, nG, size(Ẽ,2)), zeros(NT, nG, size(Ẽ,2)) + return A_Gmin, A_Gmax +end + +function custom_lincon( + model::NonLinModel{NT}, ::TranscriptionMethod, nG, Ḡy, Ḡu, Ḡd, Ḡr, Ẽ +) where {NT<:Real} + A_Gmin, A_Gmax = zeros(NT, nG, size(Ẽ,2)), zeros(NT, nG, size(Ẽ,2)) + return A_Gmin, A_Gmax +end + @doc raw""" init_matconstraint_mpc( model::LinModel, transcription::TranscriptionMethod, nc::Int, From 2bbbfd44d071c693278641f6787bf738621e6818 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Feb 2026 17:08:13 -0500 Subject: [PATCH 11/43] added: custom linear constraint almost finished --- docs/src/internals/predictive_control.md | 2 + src/controller/construct.jl | 201 ++++++++++++++++++----- src/controller/execute.jl | 19 ++- src/controller/explicitmpc.jl | 8 +- src/controller/linmpc.jl | 37 +++-- src/controller/nonlinmpc.jl | 37 +++-- src/controller/transcription.jl | 77 ++++++--- src/general.jl | 5 + src/sim_model.jl | 5 - 9 files changed, 281 insertions(+), 110 deletions(-) diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 4e8e74d17..77b9a7cbd 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -21,6 +21,8 @@ ModelPredictiveControl.relaxU ModelPredictiveControl.relaxΔU ModelPredictiveControl.relaxŶ ModelPredictiveControl.relaxterminal +ModelPredictiveControl.relaxG +ModelPredictiveControl.augmentdefect ModelPredictiveControl.init_quadprog ModelPredictiveControl.init_stochpred ModelPredictiveControl.init_matconstraint_mpc diff --git a/src/controller/construct.jl b/src/controller/construct.jl index f28c33af1..03484965d 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -7,6 +7,9 @@ struct PredictiveControllerBuffer{NT<:Real} D̂ ::Vector{NT} Ŷ ::Vector{NT} U ::Vector{NT} + D̂e::Vector{NT} + Ŷe::Vector{NT} + Ue::Vector{NT} Ẽ ::Matrix{NT} P̃u::Matrix{NT} empty::Vector{NT} @@ -22,17 +25,20 @@ The buffer is used to store intermediate results during computation without allo function PredictiveControllerBuffer( estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp::Int, Hc::Int, nϵ::Int ) where NT <: Real - nu, ny, nd, nx̂ = estim.model.nu, estim.model.ny, estim.model.nd, estim.nx̂ + nu, ny, nd = estim.model.nu, estim.model.ny, estim.model.nd nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ u = Vector{NT}(undef, nu) Z̃ = Vector{NT}(undef, nZ̃) D̂ = Vector{NT}(undef, nd*Hp) Ŷ = Vector{NT}(undef, ny*Hp) U = Vector{NT}(undef, nu*Hp) + D̂e = Vector{NT}(undef, nd*(Hp+1)) + Ŷe = Vector{NT}(undef, ny*(Hp+1)) + Ue = Vector{NT}(undef, nu*(Hp+1)) Ẽ = Matrix{NT}(undef, ny*Hp, nZ̃) P̃u = Matrix{NT}(undef, nu*Hp, nZ̃) empty = Vector{NT}(undef, 0) - return PredictiveControllerBuffer{NT}(u, Z̃, D̂, Ŷ, U, Ẽ, P̃u, empty) + return PredictiveControllerBuffer{NT}(u, Z̃, D̂, Ŷ, U, D̂e, Ŷe, Ue, Ẽ, P̃u, empty) end "Include all the objective function weights of [`PredictiveController`](@ref)" @@ -134,6 +140,13 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} Kŝ ::Matrix{NT} Vŝ ::Matrix{NT} Bŝ ::Vector{NT} + # custom linear equality constraints: + nG ::Int + FG ::Vector{NT} + Ḡy ::SparseMatrixCSC{NT, Int} + Ḡu ::SparseMatrixCSC{NT, Int} + Ḡd ::SparseMatrixCSC{NT, Int} + Ḡr ::SparseMatrixCSC{NT, Int} # bounds over the prediction horizon (deviation vectors from operating points): U0min ::Vector{NT} U0max ::Vector{NT} @@ -168,12 +181,6 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} beq ::Vector{NT} # nonlinear equality constraints: neq ::Int - # custom linear equality constraints: - Ḡy ::SparseMatrixCSC{NT, Int} - Ḡu ::SparseMatrixCSC{NT, Int} - Ḡd ::SparseMatrixCSC{NT, Int} - Ḡr ::SparseMatrixCSC{NT, Int} - nG ::Int # constraint softness parameter vectors for the nonlinear inequality constraints: C_ymin ::Vector{NT} C_ymax ::Vector{NT} @@ -296,8 +303,9 @@ LinMPC controller with a sample time Ts = 4.0 s: \mathbf{G_y ŷ}(k+H_p) + \mathbf{G_u u}(k+H_p) + \mathbf{G_d d̂}(k+H_p) + \mathbf{G_r r̂_y}(k+H_p) \end{bmatrix} ``` The matrices ``\mathbf{G_y}``, ``\mathbf{G_u}``, ``\mathbf{G_d}`` and ``\mathbf{G_r}`` - are provided at construction time and they must have the same number of rows. The terms - with ``\mathbf{G_y}`` are present only if the model is a [`LinModel`](@ref). + must have `nG` rows and are provided at construction time. The terms with + ``\mathbf{G_y}`` are present only if the model is a [`LinModel`](@ref). Any `nothing` + value for these matrices is treated as a zero matrix. """ function setconstraint!( mpc::PredictiveController; @@ -312,9 +320,11 @@ function setconstraint!( Umin = nothing, Umax = nothing, DeltaUmin = nothing, DeltaUmax = nothing, Ymin = nothing, Ymax = nothing, + Gmin = nothing, Gmax = nothing, C_umax = nothing, C_umin = nothing, C_Deltaumax = nothing, C_Deltaumin = nothing, C_ymax = nothing, C_ymin = nothing, + C_Gmax = nothing, C_Gmin = nothing, Δumin = Deltaumin, Δumax = Deltaumax, x̂min = xhatmin, x̂max = xhatmax, c_Δumin = c_Deltaumin, c_Δumax = c_Deltaumax, @@ -325,7 +335,7 @@ function setconstraint!( model, con = mpc.estim.model, mpc.con transcription, optim = mpc.transcription, mpc.optim nu, ny, nx̂, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.Hp, mpc.Hc - nϵ, nc = mpc.nϵ, con.nc + nϵ, nG, nc = mpc.nϵ, con.nG, con.nc notSolvedYet = (JuMP.termination_status(optim) == JuMP.OPTIMIZE_NOT_CALLED) if isnothing(Umin) && !isnothing(umin) size(umin) == (nu,) || throw(ArgumentError("umin size must be $((nu,))")) @@ -389,9 +399,18 @@ function setconstraint!( size(x̂max) == (nx̂,) || throw(ArgumentError("x̂max size must be $((nx̂,))")) con.x̂0max .= x̂max .- mpc.estim.x̂op end + if !isnothing(Gmin) + size(Gmin) == (nG*(Hp+1),) || throw(ArgumentError("Gmin size must be $((nG*(Hp+1),))")) + con.Gmin .= Gmin + end + if !isnothing(Gmax) + size(Gmax) == (nG*(Hp+1),) || throw(ArgumentError("Gmax size must be $((nG*(Hp+1),))")) + con.Gmax .= Gmax + end allECRs = ( c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax, C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax, c_x̂min, c_x̂max, + C_Gmin, C_Gmax ) if any(ECR -> !isnothing(ECR), allECRs) nϵ == 1 || throw(ArgumentError("Slack variable weight Cwt must be finite to set softness parameters")) @@ -406,48 +425,58 @@ function setconstraint!( isnothing(C_ymax) && !isnothing(c_ymax) && (C_ymax = repeat(c_ymax, Hp)) if !isnothing(C_umin) size(C_umin) == (nu*Hp,) || throw(ArgumentError("C_umin size must be $((nu*Hp,))")) - any(C_umin .< 0) && error("C_umin weights should be non-negative") + any(<(0), C_umin) && error("C_umin weights should be non-negative") con.A_Umin[:, end] .= -C_umin end if !isnothing(C_umax) size(C_umax) == (nu*Hp,) || throw(ArgumentError("C_umax size must be $((nu*Hp,))")) - any(C_umax .< 0) && error("C_umax weights should be non-negative") + any(<(0), C_umax) && error("C_umax weights should be non-negative") con.A_Umax[:, end] .= -C_umax end if !isnothing(C_Δumin) size(C_Δumin) == (nu*Hc,) || throw(ArgumentError("C_Δumin size must be $((nu*Hc,))")) - any(C_Δumin .< 0) && error("C_Δumin weights should be non-negative") + any(<(0), C_Δumin) && error("C_Δumin weights should be non-negative") con.A_ΔŨmin[1:end-1, end] .= -C_Δumin end if !isnothing(C_Δumax) size(C_Δumax) == (nu*Hc,) || throw(ArgumentError("C_Δumax size must be $((nu*Hc,))")) - any(C_Δumax .< 0) && error("C_Δumax weights should be non-negative") + any(<(0), C_Δumax) && error("C_Δumax weights should be non-negative") con.A_ΔŨmax[1:end-1, end] .= -C_Δumax end if !isnothing(C_ymin) size(C_ymin) == (ny*Hp,) || throw(ArgumentError("C_ymin size must be $((ny*Hp,))")) - any(C_ymin .< 0) && error("C_ymin weights should be non-negative") + any(<(0), C_ymin) && error("C_ymin weights should be non-negative") con.C_ymin .= C_ymin size(con.A_Ymin, 1) ≠ 0 && (con.A_Ymin[:, end] .= -con.C_ymin) # for LinModel end if !isnothing(C_ymax) size(C_ymax) == (ny*Hp,) || throw(ArgumentError("C_ymax size must be $((ny*Hp,))")) - any(C_ymax .< 0) && error("C_ymax weights should be non-negative") + any(<(0), C_ymax) && error("C_ymax weights should be non-negative") con.C_ymax .= C_ymax size(con.A_Ymax, 1) ≠ 0 && (con.A_Ymax[:, end] .= -con.C_ymax) # for LinModel end if !isnothing(c_x̂min) size(c_x̂min) == (nx̂,) || throw(ArgumentError("c_x̂min size must be $((nx̂,))")) - any(c_x̂min .< 0) && error("c_x̂min weights should be non-negative") + any(<(0), c_x̂min) && error("c_x̂min weights should be non-negative") con.c_x̂min .= c_x̂min size(con.A_x̂min, 1) ≠ 0 && (con.A_x̂min[:, end] .= -con.c_x̂min) # for LinModel end if !isnothing(c_x̂max) size(c_x̂max) == (nx̂,) || throw(ArgumentError("c_x̂max size must be $((nx̂,))")) - any(c_x̂max .< 0) && error("c_x̂max weights should be non-negative") + any(<(0), c_x̂max) && error("c_x̂max weights should be non-negative") con.c_x̂max .= c_x̂max size(con.A_x̂max, 1) ≠ 0 && (con.A_x̂max[:, end] .= -con.c_x̂max) # for LinModel end + if !isnothing(C_Gmin) + size(C_Gmin) == (nG*(Hp+1),) || throw(ArgumentError("C_Gmin size must be $((nG*(Hp+1),))")) + any(<(0), C_Gmin) && error("C_Gmin weights should be non-negative") + con.A_Gmin[:, end] .= -C_Gmin + end + if !isnothing(C_Gmax) + size(C_Gmax) == (nG*(Hp+1),) || throw(ArgumentError("C_Gmax size must be $((nG*(Hp+1),))")) + any(<(0), C_Gmax) && error("C_Gmax weights should be non-negative") + con.A_Gmax[:, end] .= -C_Gmax + end end i_Umin, i_Umax = .!isinf.(con.U0min), .!isinf.(con.U0max) i_ΔŨmin, i_ΔŨmax = .!isinf.(con.ΔŨmin), .!isinf.(con.ΔŨmax) @@ -497,7 +526,8 @@ Estimate the default prediction horizon `Hp` for [`LinModel`](@ref). default_Hp(model::LinModel) = DEFAULT_HP0 + estimate_delays(model) "Throw an error when model is not a [`LinModel`](@ref)." function default_Hp(::SimModel) - throw(ArgumentError("Prediction horizon Hp must be explicitly specified if model is not a LinModel.")) + msg = "Prediction horizon Hp must be explicitly specified if model is not a LinModel." + throw(ArgumentError(msg)) end """ @@ -590,21 +620,35 @@ end get_Hc(nb::AbstractVector{Int}) = length(nb) "Validate the custom linear constraint matrices dimensions." -function validate_custom_lincon(model::SimModel, Gy, Gu, Gd, Gr) +function validate_custom_lincon(model::SimModel{NT}, Gy, Gu, Gd, Gr) where NT<:Real nu, nd, ny = model.nu, model.nd, model.ny - isnothing(Gy) && (Gy = zeros(NT, 0, nx̂)) - isnothing(Gu) && (Gu = zeros(NT, 0, nu)) - isnothing(Gd) && (Gd = zeros(NT, 0, nd)) - isnothing(Gr) && (Gr = zeros(NT, 0, ny)) + if !isa(model, LinModel) && !isnothing(Gy) + throw(ArgumentError("Gy matrix can be specified only with LinModel.")) + end + nG = if !isnothing(Gy) + size(Gy, 1) + elseif !isnothing(Gu) + size(Gu, 1) + elseif !isnothing(Gd) + size(Gd, 1) + elseif !isnothing(Gr) + size(Gr, 1) + else + 0 + end + Gy = isnothing(Gy) ? zeros(NT, nG, ny) : Gy + Gu = isnothing(Gu) ? zeros(NT, nG, nu) : Gu + Gd = isnothing(Gd) ? zeros(NT, nG, nd) : Gd + Gr = isnothing(Gr) ? zeros(NT, nG, ny) : Gr size(Gy, 2) == ny || throw(DimensionMismatch("Gy must have $ny columns.")) size(Gu, 2) == nu || throw(DimensionMismatch("Gu must have $nu columns.")) size(Gd, 2) == nd || throw(DimensionMismatch("Gd must have $nd columns.")) size(Gr, 2) == ny || throw(DimensionMismatch("Gr must have $ny columns.")) - nGr = size(Gr, 1) - size(Gy, 1) == nGr || throw(DimensionMismatch("Gy must have $nGr rows.")) - size(Gu, 1) == nGr || throw(DimensionMismatch("Gu must have $nGr rows.")) - size(Gd, 1) == nGr || throw(DimensionMismatch("Gd must have $nGr rows.")) - return nothing + if size(Gy, 1) != nG || size(Gu, 1) != nG || size(Gd, 1) != nG || size(Gr, 1) != nG + msg = "all custom linear constraint matrices must have the same number of rows." + throw(DimensionMismatch(msg)) + end + return Gy, Gu, Gd, Gr, nG end """ @@ -688,8 +732,8 @@ verify_cond(::TranscriptionMethod,_,_) = nothing transcription::TranscriptionMethod, Hp, Hc, PΔu, Pu, E, - ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, + Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, Gy, Gu, Gd, Gr, gc!=nothing, nc=0 ) -> con, nϵ, P̃Δu, P̃u, Ẽ @@ -704,26 +748,26 @@ function init_defaultcon_mpc( transcription::TranscriptionMethod, Hp, Hc, PΔu, Pu, E, - ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, + Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, Gy, Gu, Gd, Gr, gc!::GCfunc = nothing, nc = 0 ) where {NT<:Real, GCfunc<:Union{Nothing, Function}} model = estim.model nu, ny, nx̂ = model.nu, model.ny, estim.nx̂ - validate_custom_lincon(model, Gy, Gu, Gd, Gr) - nG = size(Gr, 1)*(Hp+1) + nG = size(Gy, 1) + nḠ = nG*(Hp+1) nϵ = weights.isinf_C ? 0 : 1 u0min, u0max = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu) Δumin, Δumax = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu) y0min, y0max = fill(convert(NT,-Inf), ny), fill(convert(NT,+Inf), ny) x̂0min, x̂0max = fill(convert(NT,-Inf), nx̂), fill(convert(NT,+Inf), nx̂) - Gmin, Gmax = fill(convert(NT,-Inf), nG), fill(convert(NT,+Inf), nG) + Gmin, Gmax = fill(convert(NT,-Inf), nḠ), fill(convert(NT,+Inf), nḠ) c_umin, c_umax = fill(zero(NT), nu), fill(zero(NT), nu) c_Δumin, c_Δumax = fill(zero(NT), nu), fill(zero(NT), nu) c_ymin, c_ymax = fill(one(NT), ny), fill(one(NT), ny) c_x̂min, c_x̂max = fill(one(NT), nx̂), fill(one(NT), nx̂) - C_Gmin, C_Gmax = fill(one(NT), nG), fill(one(NT), nG) + C_Gmin, C_Gmax = fill(one(NT), nḠ), fill(one(NT), nḠ) U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max = repeat_constraints(Hp, Hc, u0min, u0max, Δumin, Δumax, y0min, y0max) C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax = @@ -736,7 +780,7 @@ function init_defaultcon_mpc( Ḡu = sparse(repeatdiag(Gu, Hp+1)) Ḡd = sparse(repeatdiag(Gd, Hp+1)) Ḡr = sparse(repeatdiag(Gr, Hp+1)) - A_Gmin, A_Gmax = custom_lincon(model, transcription, nG, Ḡy, Ḡu, Ḡd, Ḡr, Ẽ) + A_Gmin, A_Gmax = relaxG(model, nḠ, Ḡy, Ḡu, E, Pu, C_Gmin, C_Gmax, nϵ) A_ŝ, Ẽŝ = augmentdefect(Eŝ, nϵ) i_Umin, i_Umax = .!isinf.(U0min), .!isinf.(U0max) i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax) @@ -749,11 +793,14 @@ function init_defaultcon_mpc( A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂max, A_x̂min, A_Gmin, A_Gmax, A_ŝ ) + # dummy fx̂, FG and Fŝ vectors (updated just before optimization) + fx̂, FG, Fŝ = zeros(NT, nx̂), zeros(NT, nḠ), zeros(NT, nx̂*Hp) # dummy b and beq vectors (updated just before optimization) b, beq = zeros(NT, size(A, 1)), zeros(NT, size(Aeq, 1)) con = ControllerConstraint{NT, GCfunc}( ẽx̂ , fx̂ , gx̂ , jx̂ , kx̂ , vx̂ , bx̂ , Ẽŝ , Fŝ , Gŝ , Jŝ , Kŝ , Vŝ , Bŝ , + nG , FG , Ḡy , Ḡu , Ḡd , Ḡr , U0min , U0max , ΔŨmin , ΔŨmax , Y0min , Y0max , x̂0min , x̂0max , Gmin , Gmax , A_Umin , A_Umax , A_ΔŨmin, A_ΔŨmax , A_Ymin , A_Ymax , A_x̂min , A_x̂max , @@ -762,8 +809,6 @@ function init_defaultcon_mpc( A_ŝ , Aeq , beq , neq , - Ḡy , Ḡu , Ḡd , Ḡr , - nG, C_ymin , C_ymax , c_x̂min , c_x̂max , i_g, gc! , nc ) @@ -939,6 +984,80 @@ function relaxterminal(ex̂::AbstractMatrix{NT}, c_x̂min, c_x̂max, nϵ) where return A_x̂min, A_x̂max, ẽx̂ end +@doc raw""" + relaxG( + model::LinModel, nḠ, Ḡy, Ḡu, E, Pu, C_Gmin, C_Gmax, nϵ + ) -> A_Gmin, A_Gmax + +Construct and augment the custom linear constraints with slack variable ϵ for softening. + +By introducing the following block-diagonal matrices with ``H_p + 1`` blocks: +```math +\begin{aligned} +\mathbf{Ḡ_y} &= \mathrm{diag}(\mathbf{G_y}, \mathbf{G_y}, \dots, \mathbf{G_y}) \\ +\mathbf{Ḡ_u} &= \mathrm{diag}(\mathbf{G_u}, \mathbf{G_u}, \dots, \mathbf{G_u}) \\ +\mathbf{Ḡ_d} &= \mathrm{diag}(\mathbf{G_d}, \mathbf{G_d}, \dots, \mathbf{G_d}) \\ +\mathbf{Ḡ_r} &= \mathrm{diag}(\mathbf{G_r}, \mathbf{G_r}, \dots, \mathbf{G_r}) +\end{aligned} +``` +the ``\mathbf{G}`` vector defined in the Extended Help section of [`setconstraint!`](@ref) +can be expressed as: +```math +\mathbf{G} = \mathbf{E_G} \mathbf{Z} + \mathbf{F_G} +``` +in which: +```math +\mathbf{E_G} = \mathbf{Ḡ_y} [\begin{smallmatrix} \mathbf{0} \\ \mathbf{E} \end{smallmatrix}] + + \mathbf{Ḡ_u} [\begin{smallmatrix} \mathbf{P_u} \\ \mathbf{p_u} \end{smallmatrix}] +``` +The ``\mathbf{E}`` matrix appears in the linear output prediction equation +``\mathbf{Ŷ_0 = E Z + F}``, and ``\mathbf{P_u}``, in the conversion equation +``\mathbf{U = P_u Z + T_u u}(k-1)``. The ``\mathbf{p_u}`` matrix corresponds to the last +`nu` rows of ``\mathbf{P_u}``. The ``\mathbf{Ḡ_u}`` term assumes that ``H_c ≤ H_p``, hence +``\mathbf{Δu}(k + H_c) = \mathbf{0}`` and ``\mathbf{u}(k + H_c) = \mathbf{u}(k + H_c - 1)``. +The ``\mathbf{F_G}`` vector is updated at each control period `k` in [`linconstraint!`](@ref) +method, and is defined as: +```math +\mathbf{F_G} = + \mathbf{Ḡ_y} [\begin{smallmatrix} \mathbf{ŷ}(k) \\ \mathbf{F + Y_{op}} \end{smallmatrix}] + + \mathbf{Ḡ_u} [\begin{smallmatrix} \mathbf{T_u u}(k-1) \\ \mathbf{u}(k-1) \end{smallmatrix}] + + \mathbf{Ḡ_d} [\begin{smallmatrix} \mathbf{d}(k) \\ \mathbf{D̂} \end{smallmatrix}] + + \mathbf{Ḡ_r} [\begin{smallmatrix} \mathbf{r_y}(k) \\ \mathbf{R̂_y} \end{smallmatrix}] +``` +Denoting the decision variables augmented with the slack variable +``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, the function +returns the ``\mathbf{A}`` matrices for the inequality constraints: +```math +\begin{bmatrix} + \mathbf{A_{G_{min}}} \\ + \mathbf{A_{G_{max}}} +\end{bmatrix} \mathbf{Z̃} ≤ +\begin{bmatrix} + - \mathbf{G_{min} + F_G} \\ + + \mathbf{G_{max} - F_G} +\end{bmatrix} +``` +""" +function relaxG( + model::SimModel{NT}, nḠ, Ḡy, Ḡu, E, Pu, C_Gmin, C_Gmax, nϵ +) where {NT<:Real} + nu, ny = model.nu, model.ny + if iszero(size(E, 1)) + # model is not a LinModel, thus no Gy terms in the custom constraints: + Gy_terms = zeros(NT, nḠ, size(E, 2)) + else + Gy_terms = Ḡy*[zeros(NT, ny, size(E, 2)); E] + end + Gu_terms = Ḡu*[Pu; Pu[end-nu+1:end, :]] + EG = Gy_terms + Gu_terms + if nϵ == 1 # Z̃ = [Z; ϵ] + A_Gmin, A_Gmax = -[EG C_Gmin], [EG -C_Gmax] + else # Z̃ = Z (only hard constraints) + A_Gmin, A_Gmax = -EG, EG + end + return A_Gmin, A_Gmax +end + @doc raw""" augmentdefect(Eŝ, nϵ) -> A_ŝ, Ẽŝ diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 7f6da0cdf..2e7285ad7 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -72,7 +72,7 @@ function moveinput!( @warn "preparestate! should be called before moveinput! with current estimators" end validate_args(mpc, ry, d, lastu, D̂, R̂y, R̂u) - initpred!(mpc, mpc.estim.model, d, lastu, D̂, R̂y, R̂u) + initpred!(mpc, mpc.estim.model, ry, d, lastu, D̂, R̂y, R̂u) linconstraint!(mpc, mpc.estim.model, mpc.transcription) linconstrainteq!(mpc, mpc.estim.model, mpc.transcription) Z̃ = optim_objective!(mpc) @@ -202,7 +202,7 @@ function addinfo!(info, mpc::PredictiveController) end @doc raw""" - initpred!(mpc::PredictiveController, model::LinModel, d, lastu, D̂, R̂y, R̂u) -> nothing + initpred!(mpc::PredictiveController, model::LinModel, ry, d, lastu, D̂, R̂y, R̂u) -> nothing Init linear model prediction matrices `F, q̃, r` and current estimated output `ŷ`. @@ -221,8 +221,8 @@ They are computed with these equations using in-place operations: \end{aligned} ``` """ -function initpred!(mpc::PredictiveController, model::LinModel, d, lastu, D̂, R̂y, R̂u) - F = initpred_common!(mpc, model, d, lastu, D̂, R̂y, R̂u) +function initpred!(mpc::PredictiveController, model::LinModel, ry, d, lastu, D̂, R̂y, R̂u) + F = initpred_common!(mpc, model, ry, d, lastu, D̂, R̂y, R̂u) F .+= mpc.B # F = F + B mul!(F, mpc.K, mpc.estim.x̂0, 1, 1) # F = F + K*x̂0 mul!(F, mpc.V, mpc.lastu0, 1, 1) # F = F + V*lastu0 @@ -254,24 +254,26 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, lastu, D̂, R end @doc raw""" - initpred!(mpc::PredictiveController, model::SimModel, d, lastu, D̂, R̂y, R̂u) + initpred!(mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u) -> nothing Init `lastu0, ŷ, F, d0, D̂0, D̂e, R̂y, R̂u` vectors when model is not a [`LinModel`](@ref). """ function initpred!(mpc::PredictiveController, model::SimModel, d, lastu, D̂, R̂y, R̂u) - F = initpred_common!(mpc, model, d, lastu, D̂, R̂y, R̂u) + F = initpred_common!(mpc, model, ry, d, lastu, D̂, R̂y, R̂u) return nothing end """ - initpred_common!(mpc::PredictiveController, model::SimModel, d, lastu, D̂, R̂y, R̂u) -> F + initpred_common!(mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u) -> F Common computations of `initpred!` for all types of [`SimModel`](@ref). Will also init `mpc.F` with 0 values, or with the stochastic predictions `Ŷs` if `mpc.estim` is an [`InternalModel`](@ref). The function returns `mpc.F`. """ -function initpred_common!(mpc::PredictiveController, model::SimModel, d, lastu, D̂, R̂y, R̂u) +function initpred_common!( + mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u +) mpc.lastu0 .= lastu .- model.uop mul!(mpc.Tu_lastu0, mpc.Tu, mpc.lastu0) mpc.ŷ .= evaloutput(mpc.estim, d) @@ -281,6 +283,7 @@ function initpred_common!(mpc::PredictiveController, model::SimModel, d, lastu, mpc.D̂e[1:model.nd] .= d mpc.D̂e[model.nd+1:end] .= D̂ end + mpc.ry .= ry mpc.R̂y .= R̂y mpc.R̂u .= R̂u predictstoch!(mpc.F, mpc, mpc.estim) diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 4dfd07283..b786e5edf 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -7,6 +7,7 @@ struct ExplicitMPC{ transcription::SingleShooting Z̃::Vector{NT} ŷ::Vector{NT} + ry::Vector{NT} Hp::Int Hc::Int nϵ::Int @@ -44,7 +45,7 @@ struct ExplicitMPC{ ) where {NT<:Real, SE<:StateEstimator, CW<:ControllerWeights} model = estim.model nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂ - ŷ = copy(model.yop) # dummy vals (updated just before optimization) + ŷ, ry = copy(model.yop), copy(model.yop) # dummy vals (updated just before optimization) nϵ = 0 # no slack variable ϵ for ExplicitMPC # dummy vals (updated just before optimization): R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) @@ -54,8 +55,7 @@ struct ExplicitMPC{ PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb) E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc, nb) - # dummy val (updated just before optimization): - F = zeros(NT, ny*Hp) + F = zeros(NT, ny*Hp) # dummy value (updated just before optimization) P̃Δu, P̃u, Ẽ = PΔu, Pu, E # no slack variable ϵ for ExplicitMPC H̃ = init_quadprog(model, transcription, weights, Ẽ, P̃Δu, P̃u) # dummy vals (updated just before optimization): @@ -71,7 +71,7 @@ struct ExplicitMPC{ mpc = new{NT, SE, CW}( estim, transcription, - Z̃, ŷ, + Z̃, ŷ, ry, Hp, Hc, nϵ, nb, weights, R̂u, R̂y, diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 1e56a41c8..a53f31419 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -16,6 +16,7 @@ struct LinMPC{ con::ControllerConstraint{NT, Nothing} Z̃::Vector{NT} ŷ::Vector{NT} + ry::Vector{NT} Hp::Int Hc::Int nϵ::Int @@ -60,25 +61,25 @@ struct LinMPC{ } model = estim.model nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂ - ŷ = copy(model.yop) # dummy vals (updated just before optimization) + ŷ, ry = copy(model.yop), copy(model.yop) # dummy vals (updated just before optimization) # dummy vals (updated just before optimization): R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) lastu0 = zeros(NT, nu) + Gy, Gu, Gd, Gr, nG = validate_custom_lincon(model, Gy, Gu, Gd, Gr) validate_transcription(model, transcription) PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb) E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( model, estim, transcription, Hp, Hc, nb ) + F = zeros(NT, ny*Hp) # dummy value (updated just before optimization) Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) - # dummy vals (updated just before optimization): - F, fx̂, Fŝ = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp) con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc( estim, weights, transcription, Hp, Hc, PΔu, Pu, E, - ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, + Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, Gy, Gu, Gd, Gr ) H̃ = init_quadprog(model, transcription, weights, Ẽ, P̃Δu, P̃u) @@ -93,7 +94,7 @@ struct LinMPC{ buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ) mpc = new{NT, SE, CW, TM, JM}( estim, transcription, optim, con, - Z̃, ŷ, + Z̃, ŷ, ry, Hp, Hc, nϵ, nb, weights, R̂u, R̂y, @@ -160,11 +161,11 @@ arguments. This controller allocates memory at each time step for the optimizati - `M_Hp=Diagonal(repeat(Mwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{M}_{H_p}``. - `N_Hc=Diagonal(repeat(Nwt,Hc))` : positive semidefinite symmetric matrix ``\mathbf{N}_{H_c}``. - `L_Hp=Diagonal(repeat(Lwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{L}_{H_p}``. +- `Gy=nothing` : custom linear constraint matrix for output (see Extended Help). +- `Gu=nothing` : custom linear constraint matrix for manipulated input (see Extended Help). +- `Gd=nothing` : custom linear constraint matrix for meas. disturbance (see Extended Help). +- `Gr=nothing` : custom linear constraint matrix for output setpoint (see Extended Help). - `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only. -- `Gy=zeros(0,model.ny)` : custom linear constraint matrix for output (see Extended Help). -- `Gu=zeros(0,model.nu)` : custom linear constraint matrix for manipulated input (see Extended Help). -- `Gd=zeros(0,model.nd)` : custom linear constraint matrix for meas. disturbance (see Extended Help). -- `Gr=zeros(0,model.ny)` : custom linear constraint matrix for output setpoint (see Extended Help). - `transcription=SingleShooting()` : [`SingleShooting`](@ref) or [`MultipleShooting`](@ref). - `optim=JuMP.Model(OSQP.MathOptInterfaceOSQP.Optimizer)` : quadratic optimizer used in the predictive controller, provided as a [`JuMP.Model`](@extref) object (default to @@ -233,11 +234,11 @@ function LinMPC( M_Hp = Diagonal(repeat(Mwt, Hp)), N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))), L_Hp = Diagonal(repeat(Lwt, Hp)), + Gy = nothing, + Gu = nothing, + Gd = nothing, + Gr = nothing, Cwt = DEFAULT_CWT, - Gy = zeros(0, model.ny), - Gu = zeros(0, model.nu), - Gd = zeros(0, model.nd), - Gr = zeros(0, model.ny), transcription::ShootingMethod = DEFAULT_LINMPC_TRANSCRIPTION, optim::JuMP.GenericModel = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false), kwargs... @@ -289,11 +290,11 @@ function LinMPC( M_Hp = Diagonal(repeat(Mwt, Hp)), N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))), L_Hp = Diagonal(repeat(Lwt, Hp)), + Gy = nothing, + Gu = nothing, + Gd = nothing, + Gr = nothing, Cwt = DEFAULT_CWT, - Gy = zeros(0, estim.model.ny), - Gu = zeros(0, estim.model.nu), - Gd = zeros(0, estim.model.nd), - Gr = zeros(0, estim.model.ny), transcription::ShootingMethod = DEFAULT_LINMPC_TRANSCRIPTION, optim::JM = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false) ) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel} diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 9a5edb6dd..d693a8ac1 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -34,6 +34,7 @@ struct NonLinMPC{ oracle::Bool Z̃::Vector{NT} ŷ::Vector{NT} + ry::Vector{NT} Hp::Int Hc::Int nϵ::Int @@ -69,6 +70,7 @@ struct NonLinMPC{ buffer::PredictiveControllerBuffer{NT} function NonLinMPC{NT}( estim::SE, Hp, Hc, nb, weights::CW, + Gy, Gu, Gd, Gr, JE::JEfunc, gc!::GCfunc, nc, p::PT, transcription::TM, optim::JM, gradient::GB, jacobian::JB, hessian::HB, oracle @@ -87,26 +89,25 @@ struct NonLinMPC{ } model = estim.model nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂ - ŷ = copy(model.yop) # dummy vals (updated just before optimization) + ŷ, ry = copy(model.yop), copy(model.yop) # dummy vals (updated just before optimization) # dummy vals (updated just before optimization): R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) lastu0 = zeros(NT, nu) + Gy, Gu, Gd, Gr, nG = validate_custom_lincon(model, Gy, Gu, Gd, Gr) validate_transcription(model, transcription) PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb) E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( model, estim, transcription, Hp, Hc, nb ) + F = zeros(NT, ny*Hp) # dummy value (updated just before optimization) Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) - # dummy vals (updated just before optimization): - F, fx̂, Fŝ = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp) - Gy, Gu, Gd, Gr = zeros(0, ny), zeros(0, nu), zeros(0, nd), zeros(0, ny) # TODO: DELETE THIS !!!! con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc( estim, weights, transcription, Hp, Hc, PΔu, Pu, E, - ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, + Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, Gy, Gu, Gd, Gr, gc!, nc ) @@ -125,7 +126,7 @@ struct NonLinMPC{ mpc = new{NT, SE, CW, TM, JM, GB, JB, HB, PT, JEfunc, GCfunc}( estim, transcription, optim, con, gradient, jacobian, hessian, oracle, - Z̃, ŷ, + Z̃, ŷ, ry, Hp, Hc, nϵ, nb, weights, JE, p, @@ -209,13 +210,17 @@ This controller allocates memory at each time step for the optimization. - `N_Hc=Diagonal(repeat(Nwt,Hc))` : positive semidefinite symmetric matrix ``\mathbf{N}_{H_c}``. - `L_Hp=Diagonal(repeat(Lwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{L}_{H_p}``. - `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only. +- `Gy=nothing` : custom linear constraint matrix for output (see Extended Help). +- `Gu=nothing` : custom linear constraint matrix for manipulated input (see Extended Help). +- `Gd=nothing` : custom linear constraint matrix for meas. disturbance (see Extended Help). +- `Gr=nothing` : custom linear constraint matrix for output setpoint (see Extended Help). - `Ewt=0.0` : economic costs weight ``E`` (scalar). - `JE=(_,_,_,_)->0.0` : economic or custom cost function ``J_E(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p})``. -- `gc=(_,_,_,_,_,_)->nothing` or `gc!` : custom inequality constraint function +- `gc=(_,_,_,_,_,_)->nothing` or `gc!` : custom nonlinear inequality constraint function ``\mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ)``, mutating or not (details in Extended Help). -- `nc=0` : number of custom inequality constraints. +- `nc=0` : number of custom nonlinear inequality constraints. - `p=model.p` : ``J_E`` and ``\mathbf{g_c}`` functions parameter ``\mathbf{p}`` (any type). - `transcription=SingleShooting()` : a [`TranscriptionMethod`](@ref) for the optimization. - `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the predictive @@ -261,6 +266,9 @@ NonLinMPC controller with a sample time Ts = 10.0 s: `NonLinMPC` controllers based on [`LinModel`](@ref) compute the predictions with matrix algebra instead of a `for` loop. This feature can accelerate the optimization, especially for the constraint handling, and is not available in any other package, to my knowledge. + See [`setconstraint!`](@ref) for details about the custom linear inequality constraint + matrices `Gy`, `Gu`, `Gd` and `Gr`. The `Gy` keyword argument can be provided only if + `model` is a [`LinModel`](@ref)). The economic cost ``J_E`` and custom constraint ``\mathbf{g_c}`` functions receive the extended vectors ``\mathbf{U_e}`` (`nu*Hp+nu` elements), ``\mathbf{Ŷ_e}`` (`ny+ny*Hp` @@ -328,6 +336,10 @@ function NonLinMPC( M_Hp = Diagonal(repeat(Mwt, Hp)), N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))), L_Hp = Diagonal(repeat(Lwt, Hp)), + Gy = nothing, + Gu = nothing, + Gd = nothing, + Gr = nothing, Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, JE ::Function = (_,_,_,_) -> 0.0, @@ -347,6 +359,7 @@ function NonLinMPC( return NonLinMPC( estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gc, nc, p, M_Hp, N_Hc, L_Hp, + Gy, Gu, Gd, Gr, transcription, optim, gradient, jacobian, hessian, oracle ) end @@ -395,6 +408,10 @@ function NonLinMPC( M_Hp = Diagonal(repeat(Mwt, Hp)), N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))), L_Hp = Diagonal(repeat(Lwt, Hp)), + Gy = nothing, + Gu = nothing, + Gd = nothing, + Gr = nothing, Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, JE ::Function = (_,_,_,_) -> 0.0, @@ -424,7 +441,7 @@ function NonLinMPC( weights = ControllerWeights(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) hessian = validate_hessian(hessian, gradient, oracle, DEFAULT_NONLINMPC_HESSIAN) return NonLinMPC{NT}( - estim, Hp, Hc, nb, weights, JE, gc!, nc, p, + estim, Hp, Hc, nb, weights, Gy, Gu, Gd, Gr, JE, gc!, nc, p, transcription, optim, gradient, jacobian, hessian, oracle ) end diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 3d0eab4e0..b11aba2a0 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -670,27 +670,6 @@ function init_defectmat( return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ end -function custom_lincon( - model::LinModel{NT}, ::TranscriptionMethod, nG, Ḡy, Ḡu, Ḡd, Ḡr, Ẽ -) where {NT<:Real} - A_Gmin, A_Gmax = zeros(NT, nG, size(Ẽ,2)), zeros(NT, nG, size(Ẽ,2)) - return A_Gmin, A_Gmax -end - -function custom_lincon( - model::NonLinModel{NT}, ::SingleShooting, nG, Ḡy, Ḡu, Ḡd, Ḡr, Ẽ -) where {NT<:Real} - A_Gmin, A_Gmax = zeros(NT, nG, size(Ẽ,2)), zeros(NT, nG, size(Ẽ,2)) - return A_Gmin, A_Gmax -end - -function custom_lincon( - model::NonLinModel{NT}, ::TranscriptionMethod, nG, Ḡy, Ḡu, Ḡd, Ḡr, Ẽ -) where {NT<:Real} - A_Gmin, A_Gmax = zeros(NT, nG, size(Ẽ,2)), zeros(NT, nG, size(Ẽ,2)) - return A_Gmin, A_Gmax -end - @doc raw""" init_matconstraint_mpc( model::LinModel, transcription::TranscriptionMethod, nc::Int, @@ -794,10 +773,12 @@ Set `b` vector for the linear model inequality constraints (``\mathbf{A Z̃ ≤ Also init ``\mathbf{f_x̂} = \mathbf{g_x̂ d_0}(k) + \mathbf{j_x̂ D̂_0} + \mathbf{k_x̂ x̂_0}(k) + \mathbf{v_x̂ u_0}(k-1) + \mathbf{b_x̂}`` vector for the terminal constraints, see -[`init_predmat`](@ref). +[`init_predmat`](@ref). The ``\mathbf{F_G}`` vector for the custom linear constraints is +also updated, see [`relaxG`](@ref). """ function linconstraint!(mpc::PredictiveController, model::LinModel, ::TranscriptionMethod) nU, nΔŨ, nY = length(mpc.con.U0min), length(mpc.con.ΔŨmin), length(mpc.con.Y0min) + nḠ = length(mpc.con.Gmin) nx̂, fx̂ = mpc.estim.nx̂, mpc.con.fx̂ fx̂ .= mpc.con.bx̂ mul!(fx̂, mpc.con.kx̂, mpc.estim.x̂0, 1, 1) @@ -806,6 +787,7 @@ function linconstraint!(mpc::PredictiveController, model::LinModel, ::Transcript mul!(fx̂, mpc.con.gx̂, mpc.d0, 1, 1) mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1) end + mpc.con.nG > 0 && linconstraint_custom!(mpc, model) n = 0 mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0 n += nU @@ -822,6 +804,10 @@ function linconstraint!(mpc::PredictiveController, model::LinModel, ::Transcript mpc.con.b[(n+1):(n+nx̂)] .= @. -mpc.con.x̂0min + fx̂ n += nx̂ mpc.con.b[(n+1):(n+nx̂)] .= @. +mpc.con.x̂0max - fx̂ + n += nx̂ + mpc.con.b[(n+1):(n+nḠ)] .= @. -mpc.con.Gmin + mpc.con.FG + n += nḠ + mpc.con.b[(n+1):(n+nḠ)] .= @. +mpc.con.Gmax - mpc.con.FG if any(mpc.con.i_b) lincon = mpc.optim[:linconstraint] JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b]) @@ -831,9 +817,11 @@ end "Set `b` excluding predicted output constraints for `NonLinModel` and not `SingleShooting`." function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::TranscriptionMethod) - nU, nΔŨ, nY = length(mpc.con.U0min), length(mpc.con.ΔŨmin), length(mpc.con.Y0min) - nx̂, fx̂ = mpc.estim.nx̂, mpc.con.fx̂ + nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin) + nḠ = length(mpc.con.Gmin) + nx̂ = mpc.estim.nx̂ # here, updating fx̂ is not necessary since fx̂ = 0 + mpc.con.nG > 0 && linconstraint_custom!(mpc, model) n = 0 mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0 n += nU @@ -846,6 +834,10 @@ function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::Transcriptio mpc.con.b[(n+1):(n+nx̂)] .= @. -mpc.con.x̂0min n += nx̂ mpc.con.b[(n+1):(n+nx̂)] .= @. +mpc.con.x̂0max + n += nx̂ + mpc.con.b[(n+1):(n+nḠ)] .= @. -mpc.con.Gmin + mpc.con.FG + n += nḠ + mpc.con.b[(n+1):(n+nḠ)] .= @. +mpc.con.Gmax - mpc.con.FG if any(mpc.con.i_b) lincon = mpc.optim[:linconstraint] JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b]) @@ -855,6 +847,8 @@ end "Also exclude terminal constraints for `NonLinModel` and `SingleShooting`." function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::SingleShooting) nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin) + nḠ = length(mpc.con.Gmin) + mpc.con.nG > 0 && linconstraint_custom!(mpc, model) n = 0 mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0 n += nU @@ -863,6 +857,10 @@ function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::SingleShooti mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin n += nΔŨ mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax + n += nΔŨ + mpc.con.b[(n+1):(n+nḠ)] .= @. -mpc.con.Gmin + mpc.con.FG + n += nḠ + mpc.con.b[(n+1):(n+nḠ)] .= @. +mpc.con.Gmax - mpc.con.FG if any(mpc.con.i_b) lincon = mpc.optim[:linconstraint] @views JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b]) @@ -870,6 +868,37 @@ function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::SingleShooti return nothing end +"Init the ``\\mathbf{F_G}`` vector for the linear model custom inequality constraints." +function linconstraint_custom!(mpc::PredictiveController, model::SimModel) + ny, nu, nd, buffer = model.ny, model.nu, model.nd, mpc.buffer + FG = mpc.con.FG + Ue_term, D̂e_term, R̂e_term = buffer.Ue, buffer.D̂e, buffer.Ŷe + FG .= 0 + Ue_term[1:end-nu] .= mpc.Tu_lastu0 .+ mpc.Uop + Ue_term[end-nu+1:end] .= mpc.lastu0 .+ model.uop + mul!(FG, mpc.con.Ḡu, Ue_term, 1, 1) + if model.nd > 0 + D̂e_term[1:nd] .= mpc.d0 .+ model.dop + D̂e_term[nd+1:end] .= mpc.D̂0 .+ model.D̂op + mul!(FG, mpc.con.Ḡd, D̂e_term, 1, 1) + end + R̂e_term[1:ny] .= mpc.ry + R̂e_term[ny+1:end] .= mpc.R̂y + mul!(FG, mpc.con.Ḡr, R̂e_term, 1, 1) + return linconstraint_custom_outputs!(mpc, model) +end + +"Also include the `Ḡy` term in the custom linear constraints for [`LinModel`](@ref)." +function linconstraint_custom_outputs!(mpc::PredictiveController, model::LinModel) + Ŷe_term, FG, ny = mpc.buffer.Ŷe, mpc.con.FG, model.ny + Ŷe_term[1:ny] .= mpc.ŷ + Ŷe_term[ny+1:end] .= mpc.F .+ mpc.Yop + mul!(FG, mpc.con.Ḡy, Ŷe_term, 1, 1) + return nothing +end +linconstraint_custom_outputs!(::PredictiveController, ::SimModel) = nothing + + @doc raw""" linconstrainteq!( mpc::PredictiveController, model::LinModel, transcription::MultipleShooting diff --git a/src/general.jl b/src/general.jl index 1e994bfe2..d2c67f759 100644 --- a/src/general.jl +++ b/src/general.jl @@ -192,6 +192,11 @@ function repeat!(Y::AbstractVector, a::AbstractVector, n::Int) return Y end +"Convert vectors to single column matrices when necessary." +to_mat(A::AbstractVector, _ ...) = reshape(A, length(A), 1) +to_mat(A::AbstractMatrix, _ ...) = A +to_mat(A::Real, dims...) = fill(A, dims) + "Convert 1-element vectors and normal matrices to Hermitians." to_hermitian(A::AbstractVector) = Hermitian(reshape(A, 1, 1), :L) to_hermitian(A::AbstractMatrix) = Hermitian(A, :L) diff --git a/src/sim_model.jl b/src/sim_model.jl index a7bfa7eb9..26b638b69 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -359,11 +359,6 @@ function validate_args(model::SimModel, d, u=nothing) end end -"Convert vectors to single column matrices when necessary." -to_mat(A::AbstractVector, _ ...) = reshape(A, length(A), 1) -to_mat(A::AbstractMatrix, _ ...) = A -to_mat(A::Real, dims...) = fill(A, dims) - include("model/linmodel.jl") include("model/linearization.jl") include("model/nonlinmodel.jl") From a49e0be4717cafa426e310c2552b90654c05232a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Feb 2026 17:38:32 -0500 Subject: [PATCH 12/43] debug: correct signature --- src/controller/execute.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 2e7285ad7..c1e7befe7 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -258,8 +258,8 @@ end Init `lastu0, ŷ, F, d0, D̂0, D̂e, R̂y, R̂u` vectors when model is not a [`LinModel`](@ref). """ -function initpred!(mpc::PredictiveController, model::SimModel, d, lastu, D̂, R̂y, R̂u) - F = initpred_common!(mpc, model, ry, d, lastu, D̂, R̂y, R̂u) +function initpred!(mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u) + initpred_common!(mpc, model, ry, d, lastu, D̂, R̂y, R̂u) return nothing end From 85e10da55d28c558c5ad67cd7f5caca4ede70d43 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Feb 2026 19:46:35 -0500 Subject: [PATCH 13/43] doc: debug signature --- docs/src/internals/predictive_control.md | 2 +- src/controller/construct.jl | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 77b9a7cbd..d5f3823b6 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -33,7 +33,7 @@ ModelPredictiveControl.get_nonlincon_oracle(::NonLinMPC, ::ModelPredictiveContro ## Update Quadratic Optimization ```@docs -ModelPredictiveControl.initpred!(::PredictiveController, ::LinModel, ::Any, ::Any, ::Any, ::Any, ::Any) +ModelPredictiveControl.initpred!(::PredictiveController, ::LinModel, ::Any, ::Any, ::Any, ::Any, ::Any, ::Any) ModelPredictiveControl.linconstraint!(::PredictiveController, ::LinModel, ::TranscriptionMethod) ModelPredictiveControl.linconstrainteq! ``` diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 03484965d..ea1d7f626 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -234,7 +234,7 @@ custom linear constraints. - all the keyword arguments above but with a first capital letter, except for the terminal constraints, e.g. `Ymax` or `C_Δumin`: for time-varying constraints (see Extended Help) - `Gmin` / `Gmax` / `C_Gmin` / `C_Gmax` : custom linear bounds and softness weights over the - prediction horizon, default bounds are `±Inf` and weights are `1.0` (see Extended Help) + prediction horizon, default bounds are `±Inf` and softness weights are `1.0` (see Extended Help) # Examples ```jldoctest From 0694e2025141d7ef686e006008497ae54e126d46 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 09:21:19 -0500 Subject: [PATCH 14/43] changed: renamed exponantiated sum function W->S --- src/controller/transcription.jl | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index b11aba2a0..fde943373 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -298,12 +298,12 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). ```math \begin{aligned} \mathbf{Q}(i, m, b) &= \begin{bmatrix} - \mathbf{Ĉ W}(i-b+0)\mathbf{B̂_u} \\ - \mathbf{Ĉ W}(i-b+1)\mathbf{B̂_u} \\ + \mathbf{Ĉ S}(i-b+0)\mathbf{B̂_u} \\ + \mathbf{Ĉ S}(i-b+1)\mathbf{B̂_u} \\ \vdots \\ - \mathbf{Ĉ W}(m-b-1)\mathbf{B̂_u} + \mathbf{Ĉ S}(m-b-1)\mathbf{B̂_u} \end{bmatrix} \\ - \mathbf{W}(m) &= ∑_{ℓ=0}^m \mathbf{Â}^ℓ + \mathbf{S}(m) &= ∑_{ℓ=0}^m \mathbf{Â}^ℓ \end{aligned} ``` the prediction matrices are computed from the ``j_ℓ`` integers introduced in the @@ -332,23 +332,23 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). \mathbf{Ĉ}\mathbf{Â}^{H_p} \end{bmatrix} \\ \mathbf{V} &= \mathbf{Q}(0, H_p, 0) \\ \mathbf{B} &= \begin{bmatrix} - \mathbf{Ĉ W}(0) \\ - \mathbf{Ĉ W}(1) \\ + \mathbf{Ĉ S}(0) \\ + \mathbf{Ĉ S}(1) \\ \vdots \\ - \mathbf{Ĉ W}(H_p-1) \end{bmatrix} \mathbf{\big(f̂_{op} - x̂_{op}\big)} + \mathbf{Ĉ S}(H_p-1) \end{bmatrix} \mathbf{\big(f̂_{op} - x̂_{op}\big)} \end{aligned} ``` For the terminal constraints, the matrices are computed with: ```math \begin{aligned} \mathbf{e_x̂} &= \begin{bmatrix} - \mathbf{W}(H_p-j_0-1)\mathbf{B̂_u} & \mathbf{W}(H_p-j_1-1)\mathbf{B̂_u} & \cdots & \mathbf{W}(H_p-j_{H_c-1}-1)\mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{S}(H_p-j_0-1)\mathbf{B̂_u} & \mathbf{S}(H_p-j_1-1)\mathbf{B̂_u} & \cdots & \mathbf{S}(H_p-j_{H_c-1}-1)\mathbf{B̂_u} \end{bmatrix} \\ \mathbf{g_x̂} &= \mathbf{Â}^{H_p-1} \mathbf{B̂_d} \\ \mathbf{j_x̂} &= \begin{bmatrix} \mathbf{Â}^{H_p-2}\mathbf{B̂_d} & \mathbf{Â}^{H_p-3}\mathbf{B̂_d} & \cdots & \mathbf{0} \end{bmatrix} \\ \mathbf{k_x̂} &= \mathbf{Â}^{H_p} \\ - \mathbf{v_x̂} &= \mathbf{W}(H_p-1)\mathbf{B̂_u} \\ - \mathbf{b_x̂} &= \mathbf{W}(H_p-1)\mathbf{\big(f̂_{op} - x̂_{op}\big)} + \mathbf{v_x̂} &= \mathbf{S}(H_p-1)\mathbf{B̂_u} \\ + \mathbf{b_x̂} &= \mathbf{S}(H_p-1)\mathbf{\big(f̂_{op} - x̂_{op}\big)} \end{aligned} ``` The complex structure of the ``\mathbf{E}`` and ``\mathbf{e_x̂}`` matrices is due to the @@ -372,12 +372,12 @@ function init_predmat( jℓ_data = [0; cumsum(nb)] # introduced in move_blocking docstring # four helper functions to improve code clarity and be similar to eqs. in docstring: getpower(array3D, power) = @views array3D[:,:, power+1] - W(m) = @views Âpow_csum[:,:, m+1] + S(m) = @views Âpow_csum[:,:, m+1] jℓ(ℓ) = jℓ_data[ℓ+1] function Q!(Q, i, m, b) for ℓ=0:m-i-1 iRows = (1:ny) .+ ny*ℓ - Q[iRows, :] = Ĉ * W(i-b+ℓ) * B̂u + Q[iRows, :] = Ĉ * S(i-b+ℓ) * B̂u end return Q end @@ -389,7 +389,7 @@ function init_predmat( K[iRow,:] = Ĉ*getpower(Âpow, j) end # --- previous manipulated inputs lastu0 --- - vx̂ = W(Hp-1)*B̂u + vx̂ = S(Hp-1)*B̂u V = Matrix{NT}(undef, Hp*ny, nu) Q!(V, 0, Hp, 0) # --- decision variables Z --- @@ -404,7 +404,7 @@ function init_predmat( Q = @views E[iRow, iCol] Q!(Q, i_Q, m_Q, b_Q) end - ex̂[:, iCol] = W(Hp - jℓ(j) - 1)*B̂u + ex̂[:, iCol] = S(Hp - jℓ(j) - 1)*B̂u end # --- current measured disturbances d0 and predictions D̂0 --- gx̂ = getpower(Âpow, Hp-1)*B̂d @@ -424,11 +424,11 @@ function init_predmat( end end # --- state x̂op and state update f̂op operating points --- - coef_bx̂ = W(Hp-1) + coef_bx̂ = S(Hp-1) coef_B = Matrix{NT}(undef, ny*Hp, nx̂) for j=1:Hp iRow = (1:ny) .+ ny*(j-1) - coef_B[iRow,:] = Ĉ*W(j-1) + coef_B[iRow,:] = Ĉ*S(j-1) end f̂op_n_x̂op = estim.f̂op - estim.x̂op bx̂ = coef_bx̂ * f̂op_n_x̂op From 0e647730e359f9407543055699273618340254d8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 11:40:11 -0500 Subject: [PATCH 15/43] changed: renaming custom linear constraint G->W This is the letter that has the lowest amount of confusion. It is used in the MHE/KF but only with a hat. --- src/controller/construct.jl | 390 +++++++++++++++++--------------- src/controller/linmpc.jl | 38 ++-- src/controller/nonlinmpc.jl | 28 +-- src/controller/transcription.jl | 87 +++---- 4 files changed, 284 insertions(+), 259 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index ea1d7f626..364940cab 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -141,12 +141,13 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} Vŝ ::Matrix{NT} Bŝ ::Vector{NT} # custom linear equality constraints: - nG ::Int - FG ::Vector{NT} + ẼW ::Matrix{NT} + FW ::Vector{NT} Ḡy ::SparseMatrixCSC{NT, Int} Ḡu ::SparseMatrixCSC{NT, Int} Ḡd ::SparseMatrixCSC{NT, Int} Ḡr ::SparseMatrixCSC{NT, Int} + nw ::Int # bounds over the prediction horizon (deviation vectors from operating points): U0min ::Vector{NT} U0max ::Vector{NT} @@ -154,10 +155,10 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} ΔŨmax ::Vector{NT} Y0min ::Vector{NT} Y0max ::Vector{NT} + Wmin ::Vector{NT} + Wmax ::Vector{NT} x̂0min ::Vector{NT} x̂0max ::Vector{NT} - Gmin ::Vector{NT} - Gmax ::Vector{NT} # A matrices for the linear inequality constraints: A_Umin ::SparseMatrixCSC{NT, Int} A_Umax ::SparseMatrixCSC{NT, Int} @@ -165,10 +166,10 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} A_ΔŨmax ::SparseMatrixCSC{NT, Int} A_Ymin ::Matrix{NT} A_Ymax ::Matrix{NT} + A_Wmin ::Matrix{NT} + A_Wmax ::Matrix{NT} A_x̂min ::Matrix{NT} A_x̂max ::Matrix{NT} - A_Gmin ::Matrix{NT} - A_Gmax ::Matrix{NT} A ::Matrix{NT} # b vector for the linear inequality constraints: b ::Vector{NT} @@ -204,14 +205,16 @@ The predictive controllers support both soft and hard constraints, defined by: \mathbf{u_{min} - c_{u_{min}}} ϵ ≤&&\ \mathbf{u}(k+j) &≤ \mathbf{u_{max} + c_{u_{max}}} ϵ &&\qquad j = 0, 1 ,..., H_p - 1 \\ \mathbf{Δu_{min} - c_{Δu_{min}}} ϵ ≤&&\ \mathbf{Δu}(k+j) &≤ \mathbf{Δu_{max} + c_{Δu_{max}}} ϵ &&\qquad j = 0, 1 ,..., H_c - 1 \\ \mathbf{y_{min} - c_{y_{min}}} ϵ ≤&&\ \mathbf{ŷ}(k+j) &≤ \mathbf{y_{max} + c_{y_{max}}} ϵ &&\qquad j = 1, 2 ,..., H_p \\ - \mathbf{x̂_{min} - c_{x̂_{min}}} ϵ ≤&&\ \mathbf{x̂}_i(k+j) &≤ \mathbf{x̂_{max} + c_{x̂_{max}}} ϵ &&\qquad j = H_p + \mathbf{w_{min} - c_{w_{min}}} ϵ ≤&&\ \mathbf{w}(k+j) &≤ \mathbf{w_{max} + c_{w_{max}}} ϵ &&\qquad j = 0, 1 ,..., H_p \\ + \mathbf{x̂_{min} - c_{x̂_{min}}} ϵ ≤&&\ \mathbf{x̂}_i(k+H_p) &≤ \mathbf{x̂_{max} + c_{x̂_{max}}} ϵ &&\qquad \end{alignat*} ``` and also ``ϵ ≥ 0``. The last line is the terminal constraints applied on the states at the end of the horizon (see Extended Help). See [`MovingHorizonEstimator`](@ref) constraints -for details on bounds and softness parameters ``\mathbf{c}``. The output and terminal -constraints are all soft by default. See Extended Help for time-varying constraints and -custom linear constraints. +for details on bounds and softness parameters ``\mathbf{c}``. The penultimate line with +``\mathbf{w}`` vector is for the custom linear inequality constraints. See Extended Help for +details on custom linear and time-varying constraints. The output, custom linear and +terminal constraints are all soft by default. # Arguments !!! info @@ -226,6 +229,7 @@ custom linear constraints. - `umin=fill(-Inf,nu)` / `umax=fill(+Inf,nu)` : manipulated input bound ``\mathbf{u_{min/max}}`` - `Δumin=fill(-Inf,nu)` / `Δumax=fill(+Inf,nu)` : manipulated input increment bound ``\mathbf{Δu_{min/max}}`` - `ymin=fill(-Inf,ny)` / `ymax=fill(+Inf,ny)` : predicted output bound ``\mathbf{y_{min/max}}`` +- `wmin=fill(-Inf,nw)` / `wmax=fill(+Inf,nw)` : custom linear constraint bound ``\mathbf{w_{min/max}}`` - `x̂min=fill(-Inf,nx̂)` / `x̂max=fill(+Inf,nx̂)` : terminal constraint bound ``\mathbf{x̂_{min/max}}`` - `c_umin=fill(0.0,nu)` / `c_umax=fill(0.0,nu)` : `umin` / `umax` softness weight ``\mathbf{c_{u_{min/max}}}`` - `c_Δumin=fill(0.0,nu)` / `c_Δumax=fill(0.0,nu)` : `Δumin` / `Δumax` softness weight ``\mathbf{c_{Δu_{min/max}}}`` @@ -233,8 +237,6 @@ custom linear constraints. - `c_x̂min=fill(1.0,nx̂)` / `c_x̂max=fill(1.0,nx̂)` : `x̂min` / `x̂max` softness weight ``\mathbf{c_{x̂_{min/max}}}`` - all the keyword arguments above but with a first capital letter, except for the terminal constraints, e.g. `Ymax` or `C_Δumin`: for time-varying constraints (see Extended Help) -- `Gmin` / `Gmax` / `C_Gmin` / `C_Gmax` : custom linear bounds and softness weights over the - prediction horizon, default bounds are `±Inf` and softness weights are `1.0` (see Extended Help) # Examples ```jldoctest @@ -276,15 +278,14 @@ LinMPC controller with a sample time Ts = 4.0 s: later at runtime, set the bound to an absolute value sufficiently large when you create the controller (but different than `±Inf`). - It is also possible to specify time-varying constraints over ``H_p`` and ``H_c`` - horizons. The custom linear inequality constraints are also time-varying but over - ``H_p + 1`` steps. In such cases, they are all defined by: + It is also possible to specify time-varying constraints over the horizons. In such + cases, they are all defined by: ```math \begin{alignat*}{3} \mathbf{U_{min} - C_{u_{min}}} ϵ ≤&&\ \mathbf{U} &≤ \mathbf{U_{max} + C_{u_{max}}} ϵ \\ \mathbf{ΔU_{min} - C_{Δu_{min}}} ϵ ≤&&\ \mathbf{ΔU} &≤ \mathbf{ΔU_{max} + C_{Δu_{max}}} ϵ \\ \mathbf{Y_{min} - C_{y_{min}}} ϵ ≤&&\ \mathbf{Ŷ} &≤ \mathbf{Y_{max} + C_{y_{max}}} ϵ \\ - \mathbf{G_{min} - C_{G_{min}}} ϵ ≤&&\ \mathbf{G} &≤ \mathbf{G_{max} + C_{G_{max}}} ϵ + \mathbf{W_{min} - C_{w_{min}}} ϵ ≤&&\ \mathbf{W} &≤ \mathbf{W_{max} + C_{w_{max}}} ϵ \end{alignat*} ``` For this, use the same keyword arguments as above but with a first capital letter: @@ -292,19 +293,19 @@ LinMPC controller with a sample time Ts = 4.0 s: - `Umin` / `Umax` / `C_umin` / `C_umax` : ``\mathbf{U}`` constraints `(nu*Hp,)`. - `ΔUmin` / `ΔUmax` / `C_Δumin` / `C_Δumax` : ``\mathbf{ΔU}`` constraints `(nu*Hc,)`. - `Ymin` / `Ymax` / `C_ymin` / `C_ymax` : ``\mathbf{Ŷ}`` constraints `(ny*Hp,)`. - - `Gmin` / `Gmax` / `C_Gmin` / `C_Gmax` : custom linear constraints `(nG*(Hp+1),)`. + - `Wmin` / `Wmax` / `C_wmin` / `C_wmax` : custom linear constraints `(nw*(Hp+1),)`. The custom constraints are all gathered in the vector: ```math - \mathbf{G} = \begin{bmatrix} - \mathbf{G_y ŷ}(k+0) + \mathbf{G_u u}(k+0) + \mathbf{G_d d}(k+0) + \mathbf{G_r r_y}(k+0) \\ - \mathbf{G_y ŷ}(k+1) + \mathbf{G_u u}(k+1) + \mathbf{G_d d̂}(k+1) + \mathbf{G_r r̂_y}(k+1) \\ + \mathbf{W} = \begin{bmatrix} + \mathbf{W_y ŷ}(k+0) + \mathbf{W_u u}(k+0) + \mathbf{W_d d}(k+0) + \mathbf{W_r r_y}(k+0) \\ + \mathbf{W_y ŷ}(k+1) + \mathbf{W_u u}(k+1) + \mathbf{W_d d̂}(k+1) + \mathbf{W_r r̂_y}(k+1) \\ \vdots \\ - \mathbf{G_y ŷ}(k+H_p) + \mathbf{G_u u}(k+H_p) + \mathbf{G_d d̂}(k+H_p) + \mathbf{G_r r̂_y}(k+H_p) \end{bmatrix} + \mathbf{W_y ŷ}(k+H_p) + \mathbf{W_u u}(k+H_p) + \mathbf{W_d d̂}(k+H_p) + \mathbf{W_r r̂_y}(k+H_p) \end{bmatrix} ``` - The matrices ``\mathbf{G_y}``, ``\mathbf{G_u}``, ``\mathbf{G_d}`` and ``\mathbf{G_r}`` - must have `nG` rows and are provided at construction time. The terms with - ``\mathbf{G_y}`` are present only if the model is a [`LinModel`](@ref). Any `nothing` + The matrices ``\mathbf{W_y}``, ``\mathbf{W_u}``, ``\mathbf{W_d}`` and ``\mathbf{W_r}`` + must have `nw` rows and are provided at construction time. The terms with + ``\mathbf{W_y}`` are present only if the model is a [`LinModel`](@ref). Any `nothing` value for these matrices is treated as a zero matrix. """ function setconstraint!( @@ -312,19 +313,21 @@ function setconstraint!( umin = nothing, umax = nothing, Deltaumin = nothing, Deltaumax = nothing, ymin = nothing, ymax = nothing, + wmin = nothing, wmax = nothing, xhatmin = nothing, xhatmax = nothing, c_umin = nothing, c_umax = nothing, c_Deltaumin = nothing, c_Deltaumax = nothing, c_ymin = nothing, c_ymax = nothing, + c_wmin = nothing, c_wmax = nothing, c_xhatmin = nothing, c_xhatmax = nothing, Umin = nothing, Umax = nothing, DeltaUmin = nothing, DeltaUmax = nothing, Ymin = nothing, Ymax = nothing, - Gmin = nothing, Gmax = nothing, + Wmin = nothing, Wmax = nothing, C_umax = nothing, C_umin = nothing, C_Deltaumax = nothing, C_Deltaumin = nothing, C_ymax = nothing, C_ymin = nothing, - C_Gmax = nothing, C_Gmin = nothing, + C_wmax = nothing, C_wmin = nothing, Δumin = Deltaumin, Δumax = Deltaumax, x̂min = xhatmin, x̂max = xhatmax, c_Δumin = c_Deltaumin, c_Δumax = c_Deltaumax, @@ -335,7 +338,7 @@ function setconstraint!( model, con = mpc.estim.model, mpc.con transcription, optim = mpc.transcription, mpc.optim nu, ny, nx̂, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.Hp, mpc.Hc - nϵ, nG, nc = mpc.nϵ, con.nG, con.nc + nϵ, nw, nc = mpc.nϵ, con.nw, con.nc notSolvedYet = (JuMP.termination_status(optim) == JuMP.OPTIMIZE_NOT_CALLED) if isnothing(Umin) && !isnothing(umin) size(umin) == (nu,) || throw(ArgumentError("umin size must be $((nu,))")) @@ -379,7 +382,7 @@ function setconstraint!( con.Y0min[i] = ymin[(i-1) % ny + 1] - mpc.Yop[i] end elseif !isnothing(Ymin) - size(Ymin) == (ny*Hp,) || throw(ArgumentError("Ymin size must be $((ny*Hp,))")) + size(Ymin) == (ny*Hp,) || throw(ArgumentError("Ymin size must be $((ny*Hp,))")) con.Y0min .= Ymin .- mpc.Yop end if isnothing(Ymax) && !isnothing(ymax) @@ -388,9 +391,28 @@ function setconstraint!( con.Y0max[i] = ymax[(i-1) % ny + 1] - mpc.Yop[i] end elseif !isnothing(Ymax) - size(Ymax) == (ny*Hp,) || throw(ArgumentError("Ymax size must be $((ny*Hp,))")) + size(Ymax) == (ny*Hp,) || throw(ArgumentError("Ymax size must be $((ny*Hp,))")) con.Y0max .= Ymax .- mpc.Yop end + + if isnothing(Wmin) && !isnothing(wmin) + size(wmin) == (nw,) || throw(ArgumentError("wmin size must be $((nw,))")) + for i = 1:nw*(Hp+1) + con.Wmin[i] = wmin[(i-1) % nw + 1] + end + elseif !isnothing(Wmin) + size(Wmin) == (nw*(Hp+1),) || throw(ArgumentError("Wmin size must be $((nw*(Hp+1),))")) + con.Wmin .= Wmin + end + if isnothing(Wmax) && !isnothing(wmax) + size(wmax) == (nw,) || throw(ArgumentError("wmax size must be $((nw,))")) + for i = 1:nw*(Hp+1) + con.Wmax[i] = wmax[(i-1) % nw + 1] + end + elseif !isnothing(Wmax) + size(Wmax) == (nw*(Hp+1),) || throw(ArgumentError("Wmax size must be $((nw*(Hp+1),))")) + con.Wmax .= Wmax + end if !isnothing(x̂min) size(x̂min) == (nx̂,) || throw(ArgumentError("x̂min size must be $((nx̂,))")) con.x̂0min .= x̂min .- mpc.estim.x̂op @@ -399,18 +421,10 @@ function setconstraint!( size(x̂max) == (nx̂,) || throw(ArgumentError("x̂max size must be $((nx̂,))")) con.x̂0max .= x̂max .- mpc.estim.x̂op end - if !isnothing(Gmin) - size(Gmin) == (nG*(Hp+1),) || throw(ArgumentError("Gmin size must be $((nG*(Hp+1),))")) - con.Gmin .= Gmin - end - if !isnothing(Gmax) - size(Gmax) == (nG*(Hp+1),) || throw(ArgumentError("Gmax size must be $((nG*(Hp+1),))")) - con.Gmax .= Gmax - end allECRs = ( - c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax, - C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax, c_x̂min, c_x̂max, - C_Gmin, C_Gmax + c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax, c_wmin, c_wmax, + C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax, C_wmin, C_wmax, + c_x̂min, c_x̂max, ) if any(ECR -> !isnothing(ECR), allECRs) nϵ == 1 || throw(ArgumentError("Slack variable weight Cwt must be finite to set softness parameters")) @@ -455,6 +469,16 @@ function setconstraint!( con.C_ymax .= C_ymax size(con.A_Ymax, 1) ≠ 0 && (con.A_Ymax[:, end] .= -con.C_ymax) # for LinModel end + if !isnothing(C_wmin) + size(C_wmin) == (nw*(Hp+1),) || throw(ArgumentError("C_wmin size must be $((nw*(Hp+1),))")) + any(<(0), C_wmin) && error("C_wmin weights should be non-negative") + con.A_Gmin[:, end] .= -C_wmin + end + if !isnothing(C_wmax) + size(C_wmax) == (nw*(Hp+1),) || throw(ArgumentError("C_wmax size must be $((nw*(Hp+1),))")) + any(<(0), C_wmax) && error("C_wmax weights should be non-negative") + con.A_Gmax[:, end] .= -C_wmax + end if !isnothing(c_x̂min) size(c_x̂min) == (nx̂,) || throw(ArgumentError("c_x̂min size must be $((nx̂,))")) any(<(0), c_x̂min) && error("c_x̂min weights should be non-negative") @@ -467,31 +491,21 @@ function setconstraint!( con.c_x̂max .= c_x̂max size(con.A_x̂max, 1) ≠ 0 && (con.A_x̂max[:, end] .= -con.c_x̂max) # for LinModel end - if !isnothing(C_Gmin) - size(C_Gmin) == (nG*(Hp+1),) || throw(ArgumentError("C_Gmin size must be $((nG*(Hp+1),))")) - any(<(0), C_Gmin) && error("C_Gmin weights should be non-negative") - con.A_Gmin[:, end] .= -C_Gmin - end - if !isnothing(C_Gmax) - size(C_Gmax) == (nG*(Hp+1),) || throw(ArgumentError("C_Gmax size must be $((nG*(Hp+1),))")) - any(<(0), C_Gmax) && error("C_Gmax weights should be non-negative") - con.A_Gmax[:, end] .= -C_Gmax - end end i_Umin, i_Umax = .!isinf.(con.U0min), .!isinf.(con.U0max) i_ΔŨmin, i_ΔŨmax = .!isinf.(con.ΔŨmin), .!isinf.(con.ΔŨmax) i_Ymin, i_Ymax = .!isinf.(con.Y0min), .!isinf.(con.Y0max) + i_Wmin, i_Wmax = .!isinf.(con.Wmin), .!isinf.(con.Wmax) i_x̂min, i_x̂max = .!isinf.(con.x̂0min), .!isinf.(con.x̂0max) - i_Gmin, i_Gmax = .!isinf.(con.Gmin), .!isinf.(con.Gmax) if notSolvedYet con.i_b[:], con.i_g[:], con.A[:] = init_matconstraint_mpc( model, transcription, nc, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, - i_Ymin, i_Ymax, i_x̂min, i_x̂max, - i_Gmin, i_Gmax, + i_Ymin, i_Ymax, i_Wmin, i_Wmax, + 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.A_Gmin, con.A_Gmax, + con.A_Ymin, con.A_Ymax, con.A_Wmin, con.A_Wmax, + con.A_x̂min, con.A_x̂max, con.A_ŝ ) A = con.A[con.i_b, :] @@ -505,8 +519,8 @@ function setconstraint!( i_b, i_g = init_matconstraint_mpc( model, transcription, nc, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, - i_Ymin, i_Ymax, i_x̂min, i_x̂max, - i_Gmin, i_Gmax + i_Ymin, i_Ymax, i_Wmin, i_Wmax, + i_x̂min, i_x̂max, ) if i_b ≠ con.i_b || i_g ≠ con.i_g error("Cannot modify ±Inf constraints after calling moveinput!") @@ -620,35 +634,35 @@ end get_Hc(nb::AbstractVector{Int}) = length(nb) "Validate the custom linear constraint matrices dimensions." -function validate_custom_lincon(model::SimModel{NT}, Gy, Gu, Gd, Gr) where NT<:Real +function validate_custom_lincon(model::SimModel{NT}, Wy, Wu, Wd, Wr) where NT<:Real nu, nd, ny = model.nu, model.nd, model.ny - if !isa(model, LinModel) && !isnothing(Gy) - throw(ArgumentError("Gy matrix can be specified only with LinModel.")) + if !isa(model, LinModel) && !isnothing(Wy) + throw(ArgumentError("Wy matrix can be specified only with LinModel.")) end - nG = if !isnothing(Gy) - size(Gy, 1) - elseif !isnothing(Gu) - size(Gu, 1) - elseif !isnothing(Gd) - size(Gd, 1) - elseif !isnothing(Gr) - size(Gr, 1) + nw = if !isnothing(Wy) + size(Wy, 1) + elseif !isnothing(Wu) + size(Wu, 1) + elseif !isnothing(Wd) + size(Wd, 1) + elseif !isnothing(Wr) + size(Wr, 1) else 0 end - Gy = isnothing(Gy) ? zeros(NT, nG, ny) : Gy - Gu = isnothing(Gu) ? zeros(NT, nG, nu) : Gu - Gd = isnothing(Gd) ? zeros(NT, nG, nd) : Gd - Gr = isnothing(Gr) ? zeros(NT, nG, ny) : Gr - size(Gy, 2) == ny || throw(DimensionMismatch("Gy must have $ny columns.")) - size(Gu, 2) == nu || throw(DimensionMismatch("Gu must have $nu columns.")) - size(Gd, 2) == nd || throw(DimensionMismatch("Gd must have $nd columns.")) - size(Gr, 2) == ny || throw(DimensionMismatch("Gr must have $ny columns.")) - if size(Gy, 1) != nG || size(Gu, 1) != nG || size(Gd, 1) != nG || size(Gr, 1) != nG + Wy = isnothing(Wy) ? zeros(NT, nw, ny) : Wy + Wu = isnothing(Wu) ? zeros(NT, nw, nu) : Wu + Wd = isnothing(Wd) ? zeros(NT, nw, nd) : Wd + Wr = isnothing(Wr) ? zeros(NT, nw, ny) : Wr + size(Wy, 2) == ny || throw(DimensionMismatch("Wy must have $ny columns.")) + size(Wu, 2) == nu || throw(DimensionMismatch("Wu must have $nu columns.")) + size(Wd, 2) == nd || throw(DimensionMismatch("Wd must have $nd columns.")) + size(Wr, 2) == ny || throw(DimensionMismatch("Wr must have $ny columns.")) + if size(Wy, 1) != nw || size(Wu, 1) != nw || size(Wd, 1) != nw || size(Wr, 1) != nw msg = "all custom linear constraint matrices must have the same number of rows." throw(DimensionMismatch(msg)) end - return Gy, Gu, Gd, Gr, nG + return Wy, Wu, Wd, Wr end """ @@ -734,7 +748,7 @@ verify_cond(::TranscriptionMethod,_,_) = nothing PΔu, Pu, E, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, - Gy, Gu, Gd, Gr, + Wy, Wu, Wd, Wr, gc!=nothing, nc=0 ) -> con, nϵ, P̃Δu, P̃u, Ẽ @@ -750,80 +764,87 @@ function init_defaultcon_mpc( PΔu, Pu, E, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, - Gy, Gu, Gd, Gr, + Wy, Wu, Wd, Wr, gc!::GCfunc = nothing, nc = 0 ) where {NT<:Real, GCfunc<:Union{Nothing, Function}} model = estim.model nu, ny, nx̂ = model.nu, model.ny, estim.nx̂ - nG = size(Gy, 1) - nḠ = nG*(Hp+1) + nw = size(Wy, 1) + nW = nw*(Hp+1) nϵ = weights.isinf_C ? 0 : 1 u0min, u0max = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu) Δumin, Δumax = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu) y0min, y0max = fill(convert(NT,-Inf), ny), fill(convert(NT,+Inf), ny) + wmin, wmax = fill(convert(NT,-Inf), nw), fill(convert(NT,+Inf), nw) x̂0min, x̂0max = fill(convert(NT,-Inf), nx̂), fill(convert(NT,+Inf), nx̂) - Gmin, Gmax = fill(convert(NT,-Inf), nḠ), fill(convert(NT,+Inf), nḠ) c_umin, c_umax = fill(zero(NT), nu), fill(zero(NT), nu) c_Δumin, c_Δumax = fill(zero(NT), nu), fill(zero(NT), nu) c_ymin, c_ymax = fill(one(NT), ny), fill(one(NT), ny) + c_wmin, c_wmax = fill(one(NT), nw), fill(one(NT), nw) c_x̂min, c_x̂max = fill(one(NT), nx̂), fill(one(NT), nx̂) - C_Gmin, C_Gmax = fill(one(NT), nḠ), fill(one(NT), nḠ) - U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max = - repeat_constraints(Hp, Hc, u0min, u0max, Δumin, Δumax, y0min, y0max) - C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax = - repeat_constraints(Hp, Hc, c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax) + U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max, Wmin, Wmax = + repeat_constraints( + Hp, Hc, u0min, u0max, Δumin, Δumax, y0min, y0max, wmin, wmax + ) + C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax, C_wmin, C_wmax = + repeat_constraints( + Hp, Hc, c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax, c_wmin, c_wmax + ) + W̄y = sparse(repeatdiag(Wy, Hp+1)) + W̄u = sparse(repeatdiag(Wu, Hp+1)) + W̄d = sparse(repeatdiag(Wd, Hp+1)) + W̄r = sparse(repeatdiag(Wr, Hp+1)) A_Umin, A_Umax, P̃u = relaxU(Pu, C_umin, C_umax, nϵ) A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃Δu = relaxΔU(PΔu, C_Δumin, C_Δumax, ΔUmin, ΔUmax, nϵ) A_Ymin, A_Ymax, Ẽ = relaxŶ(E, C_ymin, C_ymax, nϵ) + A_Wmin, A_Wmax, ẼW = relaxW(E, Pu, Hp, W̄y, W̄u, C_wmin, C_wmax, nϵ) A_x̂min, A_x̂max, ẽx̂ = relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ) - Ḡy = sparse(repeatdiag(Gy, Hp+1)) - Ḡu = sparse(repeatdiag(Gu, Hp+1)) - Ḡd = sparse(repeatdiag(Gd, Hp+1)) - Ḡr = sparse(repeatdiag(Gr, Hp+1)) - A_Gmin, A_Gmax = relaxG(model, nḠ, Ḡy, Ḡu, E, Pu, C_Gmin, C_Gmax, nϵ) A_ŝ, Ẽŝ = augmentdefect(Eŝ, nϵ) i_Umin, i_Umax = .!isinf.(U0min), .!isinf.(U0max) i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax) i_Ymin, i_Ymax = .!isinf.(Y0min), .!isinf.(Y0max) + i_Wmin, i_Wmax = .!isinf.(Wmin), .!isinf.(Wmax) i_x̂min, i_x̂max = .!isinf.(x̂0min), .!isinf.(x̂0max) - i_Gmin, i_Gmax = .!isinf.(Gmin), .!isinf.(Gmax) 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, i_Gmin, i_Gmax, - A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂max, A_x̂min, A_Gmin, A_Gmax, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_Wmin, i_Wmax, i_x̂min, i_x̂max, + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂max, A_x̂min, A_ŝ ) # dummy fx̂, FG and Fŝ vectors (updated just before optimization) - fx̂, FG, Fŝ = zeros(NT, nx̂), zeros(NT, nḠ), zeros(NT, nx̂*Hp) + fx̂, FW, Fŝ = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nx̂*Hp) # dummy b and beq vectors (updated just before optimization) b, beq = zeros(NT, size(A, 1)), zeros(NT, size(Aeq, 1)) con = ControllerConstraint{NT, GCfunc}( - ẽx̂ , fx̂ , gx̂ , jx̂ , kx̂ , vx̂ , bx̂ , - Ẽŝ , Fŝ , Gŝ , Jŝ , Kŝ , Vŝ , Bŝ , - nG , FG , Ḡy , Ḡu , Ḡd , Ḡr , - U0min , U0max , ΔŨmin , ΔŨmax , Y0min , Y0max , x̂0min , x̂0max , - Gmin , Gmax , - A_Umin , A_Umax , A_ΔŨmin, A_ΔŨmax , A_Ymin , A_Ymax , A_x̂min , A_x̂max , - A_Gmin , A_Gmax , - A , b , i_b , + ẽx̂ , fx̂ , gx̂ , jx̂ , kx̂ , vx̂ , bx̂ , + Ẽŝ , Fŝ , Gŝ , Jŝ , Kŝ , Vŝ , Bŝ , + ẼW , FW , W̄y , W̄u , W̄d , W̄r , nw , + U0min , U0max , ΔŨmin , ΔŨmax , + Y0min , Y0max , Wmin , Wmax , x̂0min , x̂0max , + A_Umin , A_Umax , A_ΔŨmin , A_ΔŨmax , + A_Ymin , A_Ymax , A_Wmin , A_Wmax , A_x̂min , A_x̂max , + A , b , i_b , A_ŝ , Aeq , beq , neq , - C_ymin , C_ymax , c_x̂min , c_x̂max , i_g, + C_ymin , C_ymax , c_x̂min , c_x̂max , + i_g , gc! , nc ) return con, nϵ, P̃Δu, P̃u, Ẽ end -"Repeat predictive controller constraints over prediction `Hp` and control `Hc` horizons." -function repeat_constraints(Hp, Hc, umin, umax, Δumin, Δumax, ymin, ymax) +"Repeat predictive controller constraints over their respective horizons." +function repeat_constraints(Hp, Hc, umin, umax, Δumin, Δumax, ymin, ymax, wmin, wmax) Umin = repeat(umin, Hp) Umax = repeat(umax, Hp) ΔUmin = repeat(Δumin, Hc) ΔUmax = repeat(Δumax, Hc) Ymin = repeat(ymin, Hp) Ymax = repeat(ymax, Hp) - return Umin, Umax, ΔUmin, ΔUmax, Ymin, Ymax + Wmin = repeat(wmin, Hp+1) + Wmax = repeat(wmax, Hp+1) + return Umin, Umax, ΔUmin, ΔUmax, Ymin, Ymax, Wmin, Wmax end @doc raw""" @@ -946,116 +967,119 @@ function relaxŶ(E::AbstractMatrix{NT}, C_ymin, C_ymax, nϵ) where NT<:Real end @doc raw""" - relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ) -> A_x̂min, A_x̂max, ẽx̂ - -Augment terminal state constraints with slack variable ϵ for softening. - -Denoting the decision variables augmented with the slack variable -``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, it returns the -``\mathbf{ẽ_{x̂}}`` matrix that appears in the terminal state equation -``\mathbf{x̂_0}(k + H_p) = \mathbf{ẽ_x̂ Z̃ + f_x̂}``, and the ``\mathbf{A}`` matrices for -the inequality constraints: -```math -\begin{bmatrix} - \mathbf{A_{x̂_{min}}} \\ - \mathbf{A_{x̂_{max}}} -\end{bmatrix} \mathbf{Z̃} ≤ -\begin{bmatrix} - - \mathbf{(x̂_{min} - x̂_{op}) + f_x̂} \\ - + \mathbf{(x̂_{max} - x̂_{op}) - f_x̂} -\end{bmatrix} -``` -""" -function relaxterminal(ex̂::AbstractMatrix{NT}, c_x̂min, c_x̂max, nϵ) where {NT<:Real} - if nϵ == 1 # Z̃ = [Z; ϵ] - if iszero(size(ex̂, 1)) - # model is not a LinModel and transcription is a SingleShooting, thus terminal - # state constraints are not linear: - c_x̂min = c_x̂max = zeros(NT, 0, 1) - end - # ϵ impacts terminal state constraint calculations: - A_x̂min, A_x̂max = -[ex̂ c_x̂min], [ex̂ -c_x̂max] - # ϵ has no impact on terminal state predictions: - ẽx̂ = [ex̂ zeros(NT, size(ex̂, 1), 1)] - else # Z̃ = Z (only hard constraints) - ẽx̂ = ex̂ - A_x̂min, A_x̂max = -ex̂, ex̂ - end - return A_x̂min, A_x̂max, ẽx̂ -end - -@doc raw""" - relaxG( - model::LinModel, nḠ, Ḡy, Ḡu, E, Pu, C_Gmin, C_Gmax, nϵ - ) -> A_Gmin, A_Gmax + relaxW(E, Pu, nw, W̄y, W̄u, C_wmin, C_wmax, nϵ) -> A_Wmin, A_Wmax, ẼW Construct and augment the custom linear constraints with slack variable ϵ for softening. By introducing the following block-diagonal matrices with ``H_p + 1`` blocks: ```math \begin{aligned} -\mathbf{Ḡ_y} &= \mathrm{diag}(\mathbf{G_y}, \mathbf{G_y}, \dots, \mathbf{G_y}) \\ -\mathbf{Ḡ_u} &= \mathrm{diag}(\mathbf{G_u}, \mathbf{G_u}, \dots, \mathbf{G_u}) \\ -\mathbf{Ḡ_d} &= \mathrm{diag}(\mathbf{G_d}, \mathbf{G_d}, \dots, \mathbf{G_d}) \\ -\mathbf{Ḡ_r} &= \mathrm{diag}(\mathbf{G_r}, \mathbf{G_r}, \dots, \mathbf{G_r}) +\mathbf{W̄_y} &= \mathrm{diag}(\mathbf{W_y}, \mathbf{W_y}, \dots, \mathbf{W_y}) \\ +\mathbf{W̄_u} &= \mathrm{diag}(\mathbf{W_u}, \mathbf{W_u}, \dots, \mathbf{W_u}) \\ +\mathbf{W̄_d} &= \mathrm{diag}(\mathbf{W_d}, \mathbf{W_d}, \dots, \mathbf{W_d}) \\ +\mathbf{W̄_r} &= \mathrm{diag}(\mathbf{W_r}, \mathbf{W_r}, \dots, \mathbf{W_r}) \end{aligned} ``` -the ``\mathbf{G}`` vector defined in the Extended Help section of [`setconstraint!`](@ref) +the ``\mathbf{W}`` vector defined in the Extended Help section of [`setconstraint!`](@ref) can be expressed as: ```math -\mathbf{G} = \mathbf{E_G} \mathbf{Z} + \mathbf{F_G} +\mathbf{W} = \mathbf{E_W} \mathbf{Z} + \mathbf{F_W} ``` in which: ```math -\mathbf{E_G} = \mathbf{Ḡ_y} [\begin{smallmatrix} \mathbf{0} \\ \mathbf{E} \end{smallmatrix}] + - \mathbf{Ḡ_u} [\begin{smallmatrix} \mathbf{P_u} \\ \mathbf{p_u} \end{smallmatrix}] +\mathbf{E_W} = \mathbf{W̄_y} [\begin{smallmatrix} \mathbf{0} \\ \mathbf{E} \end{smallmatrix}] + + \mathbf{W̄_u} [\begin{smallmatrix} \mathbf{P_u} \\ \mathbf{p_u} \end{smallmatrix}] ``` The ``\mathbf{E}`` matrix appears in the linear output prediction equation ``\mathbf{Ŷ_0 = E Z + F}``, and ``\mathbf{P_u}``, in the conversion equation ``\mathbf{U = P_u Z + T_u u}(k-1)``. The ``\mathbf{p_u}`` matrix corresponds to the last -`nu` rows of ``\mathbf{P_u}``. The ``\mathbf{Ḡ_u}`` term assumes that ``H_c ≤ H_p``, hence +`nu` rows of ``\mathbf{P_u}``. The ``\mathbf{W̄_u}`` term assumes that ``H_c ≤ H_p``, hence ``\mathbf{Δu}(k + H_c) = \mathbf{0}`` and ``\mathbf{u}(k + H_c) = \mathbf{u}(k + H_c - 1)``. -The ``\mathbf{F_G}`` vector is updated at each control period `k` in [`linconstraint!`](@ref) +The ``\mathbf{F_W}`` vector is updated at each control period `k` in [`linconstraint!`](@ref) method, and is defined as: ```math -\mathbf{F_G} = - \mathbf{Ḡ_y} [\begin{smallmatrix} \mathbf{ŷ}(k) \\ \mathbf{F + Y_{op}} \end{smallmatrix}] + - \mathbf{Ḡ_u} [\begin{smallmatrix} \mathbf{T_u u}(k-1) \\ \mathbf{u}(k-1) \end{smallmatrix}] + - \mathbf{Ḡ_d} [\begin{smallmatrix} \mathbf{d}(k) \\ \mathbf{D̂} \end{smallmatrix}] + - \mathbf{Ḡ_r} [\begin{smallmatrix} \mathbf{r_y}(k) \\ \mathbf{R̂_y} \end{smallmatrix}] +\mathbf{F_W} = + \mathbf{W̄_y} [\begin{smallmatrix} \mathbf{ŷ}(k) \\ \mathbf{F + Y_{op}} \end{smallmatrix}] + + \mathbf{W̄_u} [\begin{smallmatrix} \mathbf{T_u u}(k-1) \\ \mathbf{u}(k-1) \end{smallmatrix}] + + \mathbf{W̄_d} [\begin{smallmatrix} \mathbf{d}(k) \\ \mathbf{D̂} \end{smallmatrix}] + + \mathbf{W̄_r} [\begin{smallmatrix} \mathbf{r_y}(k) \\ \mathbf{R̂_y} \end{smallmatrix}] ``` Denoting the decision variables augmented with the slack variable ``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, the function -returns the ``\mathbf{A}`` matrices for the inequality constraints: +returns the ``\mathbf{Ẽ_W}`` matrix that appears in the custom constraint equation +``\mathbf{W = Ẽ_W Z̃ + F_W}``, and the ``\mathbf{A}`` matrices for the inequality constraints: ```math \begin{bmatrix} - \mathbf{A_{G_{min}}} \\ - \mathbf{A_{G_{max}}} + \mathbf{A_{W_{min}}} \\ + \mathbf{A_{W_{max}}} \end{bmatrix} \mathbf{Z̃} ≤ \begin{bmatrix} - - \mathbf{G_{min} + F_G} \\ - + \mathbf{G_{max} - F_G} + - \mathbf{W_{min} + F_W} \\ + + \mathbf{W_{max} - F_W} \end{bmatrix} ``` """ -function relaxG( - model::SimModel{NT}, nḠ, Ḡy, Ḡu, E, Pu, C_Gmin, C_Gmax, nϵ -) where {NT<:Real} - nu, ny = model.nu, model.ny +function relaxW(E::AbstractMatrix{NT}, Pu, Hp, W̄y, W̄u, C_wmin, C_wmax, nϵ) where {NT<:Real} + nW = size(W̄y, 1) + ny = size(W̄y, 2) ÷ (Hp + 1) + nu = size(Pu, 1) ÷ Hp if iszero(size(E, 1)) - # model is not a LinModel, thus no Gy terms in the custom constraints: - Gy_terms = zeros(NT, nḠ, size(E, 2)) + # model is not a LinModel, thus no Wy terms in the custom constraints: + Wy_terms = zeros(NT, nW, size(E, 2)) else - Gy_terms = Ḡy*[zeros(NT, ny, size(E, 2)); E] + Wy_terms = W̄y*[zeros(NT, ny, size(E, 2)); E] + end + Wu_terms = W̄u*[Pu; Pu[end-nu+1:end, :]] + EW = Wy_terms + Wu_terms + if nϵ == 1 # Z̃ = [Z; ϵ] + # ϵ impacts custom constraint calculations: + A_Wmin, A_Wmax = -[EW C_wmin], [EW -C_wmax] + # ϵ has no impact on custom constraint predictions: + ẼW = [EW zeros(NT, nW, 1)] + else # Z̃ = Z (only hard constraints) + ẼW = EW + A_Wmin, A_Wmax = -EW, EW end - Gu_terms = Ḡu*[Pu; Pu[end-nu+1:end, :]] - EG = Gy_terms + Gu_terms + return A_Wmin, A_Wmax, ẼW +end + +@doc raw""" + relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ) -> A_x̂min, A_x̂max, ẽx̂ + +Augment terminal state constraints with slack variable ϵ for softening. + +Denoting the decision variables augmented with the slack variable +``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, it returns the +``\mathbf{ẽ_{x̂}}`` matrix that appears in the terminal state equation +``\mathbf{x̂_0}(k + H_p) = \mathbf{ẽ_x̂ Z̃ + f_x̂}``, and the ``\mathbf{A}`` matrices for +the inequality constraints: +```math +\begin{bmatrix} + \mathbf{A_{x̂_{min}}} \\ + \mathbf{A_{x̂_{max}}} +\end{bmatrix} \mathbf{Z̃} ≤ +\begin{bmatrix} + - \mathbf{(x̂_{min} - x̂_{op}) + f_x̂} \\ + + \mathbf{(x̂_{max} - x̂_{op}) - f_x̂} +\end{bmatrix} +``` +""" +function relaxterminal(ex̂::AbstractMatrix{NT}, c_x̂min, c_x̂max, nϵ) where {NT<:Real} if nϵ == 1 # Z̃ = [Z; ϵ] - A_Gmin, A_Gmax = -[EG C_Gmin], [EG -C_Gmax] + if iszero(size(ex̂, 1)) + # model is not a LinModel and transcription is a SingleShooting, thus terminal + # state constraints are not linear: + c_x̂min = c_x̂max = zeros(NT, 0, 1) + end + # ϵ impacts terminal state constraint calculations: + A_x̂min, A_x̂max = -[ex̂ c_x̂min], [ex̂ -c_x̂max] + # ϵ has no impact on terminal state predictions: + ẽx̂ = [ex̂ zeros(NT, size(ex̂, 1), 1)] else # Z̃ = Z (only hard constraints) - A_Gmin, A_Gmax = -EG, EG + ẽx̂ = ex̂ + A_x̂min, A_x̂max = -ex̂, ex̂ end - return A_Gmin, A_Gmax + return A_x̂min, A_x̂max, ẽx̂ end @doc raw""" diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index a53f31419..47424d799 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -50,7 +50,7 @@ struct LinMPC{ buffer::PredictiveControllerBuffer{NT} function LinMPC{NT}( estim::SE, Hp, Hc, nb, weights::CW, - Gy, Gu, Gd, Gr, + Wy, Wu, Wd, Wr, transcription::TM, optim::JM ) where { NT<:Real, @@ -60,12 +60,12 @@ struct LinMPC{ JM<:JuMP.GenericModel } model = estim.model - nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂ + nu, ny, nd = model.nu, model.ny, model.nd ŷ, ry = copy(model.yop), copy(model.yop) # dummy vals (updated just before optimization) # dummy vals (updated just before optimization): R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) lastu0 = zeros(NT, nu) - Gy, Gu, Gd, Gr, nG = validate_custom_lincon(model, Gy, Gu, Gd, Gr) + Wy, Wu, Wd, Wr = validate_custom_lincon(model, Wy, Wu, Wd, Wr) validate_transcription(model, transcription) PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb) @@ -80,7 +80,7 @@ struct LinMPC{ PΔu, Pu, E, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, - Gy, Gu, Gd, Gr + Wy, Wu, Wd, Wr ) H̃ = init_quadprog(model, transcription, weights, Ẽ, P̃Δu, P̃u) # dummy vals (updated just before optimization): @@ -161,10 +161,10 @@ arguments. This controller allocates memory at each time step for the optimizati - `M_Hp=Diagonal(repeat(Mwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{M}_{H_p}``. - `N_Hc=Diagonal(repeat(Nwt,Hc))` : positive semidefinite symmetric matrix ``\mathbf{N}_{H_c}``. - `L_Hp=Diagonal(repeat(Lwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{L}_{H_p}``. -- `Gy=nothing` : custom linear constraint matrix for output (see Extended Help). -- `Gu=nothing` : custom linear constraint matrix for manipulated input (see Extended Help). -- `Gd=nothing` : custom linear constraint matrix for meas. disturbance (see Extended Help). -- `Gr=nothing` : custom linear constraint matrix for output setpoint (see Extended Help). +- `Wy=nothing` : custom linear constraint matrix for output (see Extended Help). +- `Wu=nothing` : custom linear constraint matrix for manipulated input (see Extended Help). +- `Wd=nothing` : custom linear constraint matrix for meas. disturbance (see Extended Help). +- `Wr=nothing` : custom linear constraint matrix for output setpoint (see Extended Help). - `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only. - `transcription=SingleShooting()` : [`SingleShooting`](@ref) or [`MultipleShooting`](@ref). - `optim=JuMP.Model(OSQP.MathOptInterfaceOSQP.Optimizer)` : quadratic optimizer used in @@ -199,7 +199,7 @@ LinMPC controller with a sample time Ts = 4.0 s: for over-actuated systems, when `nu > ny` (e.g. prioritize solutions with lower economical costs). The default `Lwt` value implies that this feature is disabled by default. - The custom linear constraint matrices `Gy`, `Gu`, `Gd`, and `Gr` allow to define + The custom linear constraint matrices `Wy`, `Wu`, `Wd`, and `Wr` allow to define constraints based on linear combinations of outputs, manipulated inputs, measured disturbances, and output setpoints, respectively. See the Extended Help section in [`setconstraint!`](@ref) documentation for more details. @@ -234,10 +234,10 @@ function LinMPC( M_Hp = Diagonal(repeat(Mwt, Hp)), N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))), L_Hp = Diagonal(repeat(Lwt, Hp)), - Gy = nothing, - Gu = nothing, - Gd = nothing, - Gr = nothing, + Wy = nothing, + Wu = nothing, + Wd = nothing, + Wr = nothing, Cwt = DEFAULT_CWT, transcription::ShootingMethod = DEFAULT_LINMPC_TRANSCRIPTION, optim::JuMP.GenericModel = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false), @@ -246,7 +246,7 @@ function LinMPC( estim = SteadyKalmanFilter(model; kwargs...) return LinMPC( estim; - Hp, Hc, Mwt, Nwt, Lwt, Cwt, M_Hp, N_Hc, L_Hp, Gy, Gu, Gd, Gr, transcription, optim + Hp, Hc, Mwt, Nwt, Lwt, Cwt, M_Hp, N_Hc, L_Hp, Wy, Wu, Wd, Wr, transcription, optim ) end @@ -290,10 +290,10 @@ function LinMPC( M_Hp = Diagonal(repeat(Mwt, Hp)), N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))), L_Hp = Diagonal(repeat(Lwt, Hp)), - Gy = nothing, - Gu = nothing, - Gd = nothing, - Gr = nothing, + Wy = nothing, + Wu = nothing, + Wd = nothing, + Wr = nothing, Cwt = DEFAULT_CWT, transcription::ShootingMethod = DEFAULT_LINMPC_TRANSCRIPTION, optim::JM = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false) @@ -307,7 +307,7 @@ function LinMPC( nb = move_blocking(Hp, Hc) Hc = get_Hc(nb) weights = ControllerWeights(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt) - return LinMPC{NT}(estim, Hp, Hc, nb, weights, Gy, Gu, Gd, Gr, transcription, optim) + return LinMPC{NT}(estim, Hp, Hc, nb, weights, Wy, Wu, Wd, Wr, transcription, optim) end """ diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index d693a8ac1..f6e69d6fb 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -70,7 +70,7 @@ struct NonLinMPC{ buffer::PredictiveControllerBuffer{NT} function NonLinMPC{NT}( estim::SE, Hp, Hc, nb, weights::CW, - Gy, Gu, Gd, Gr, + Wy, Wu, Wd, Wr, JE::JEfunc, gc!::GCfunc, nc, p::PT, transcription::TM, optim::JM, gradient::GB, jacobian::JB, hessian::HB, oracle @@ -88,12 +88,12 @@ struct NonLinMPC{ GCfunc<:Function, } model = estim.model - nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂ + nu, ny, nd = model.nu, model.ny, model.nd ŷ, ry = copy(model.yop), copy(model.yop) # dummy vals (updated just before optimization) # dummy vals (updated just before optimization): R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) lastu0 = zeros(NT, nu) - Gy, Gu, Gd, Gr, nG = validate_custom_lincon(model, Gy, Gu, Gd, Gr) + Wy, Wu, Wd, Wr = validate_custom_lincon(model, Wy, Wu, Wd, Wr) validate_transcription(model, transcription) PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb) @@ -108,7 +108,7 @@ struct NonLinMPC{ PΔu, Pu, E, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, - Gy, Gu, Gd, Gr, + Wy, Wu, Wd, Wr, gc!, nc ) warn_cond = iszero(weights.E) ? 1e6 : Inf # condition number warning only if Ewt==0 @@ -336,10 +336,10 @@ function NonLinMPC( M_Hp = Diagonal(repeat(Mwt, Hp)), N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))), L_Hp = Diagonal(repeat(Lwt, Hp)), - Gy = nothing, - Gu = nothing, - Gd = nothing, - Gr = nothing, + Wy = nothing, + Wu = nothing, + Wd = nothing, + Wr = nothing, Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, JE ::Function = (_,_,_,_) -> 0.0, @@ -359,7 +359,7 @@ function NonLinMPC( return NonLinMPC( estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gc, nc, p, M_Hp, N_Hc, L_Hp, - Gy, Gu, Gd, Gr, + Wy, Wu, Wd, Wr, transcription, optim, gradient, jacobian, hessian, oracle ) end @@ -408,10 +408,10 @@ function NonLinMPC( M_Hp = Diagonal(repeat(Mwt, Hp)), N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))), L_Hp = Diagonal(repeat(Lwt, Hp)), - Gy = nothing, - Gu = nothing, - Gd = nothing, - Gr = nothing, + Wy = nothing, + Wu = nothing, + Wd = nothing, + Wr = nothing, Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, JE ::Function = (_,_,_,_) -> 0.0, @@ -441,7 +441,7 @@ function NonLinMPC( weights = ControllerWeights(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) hessian = validate_hessian(hessian, gradient, oracle, DEFAULT_NONLINMPC_HESSIAN) return NonLinMPC{NT}( - estim, Hp, Hc, nb, weights, Gy, Gu, Gd, Gr, JE, gc!, nc, p, + estim, Hp, Hc, nb, weights, Wy, Wu, Wd, Wr, JE, gc!, nc, p, transcription, optim, gradient, jacobian, hessian, oracle ) end diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index fde943373..be406f5ad 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -673,7 +673,7 @@ end @doc raw""" init_matconstraint_mpc( model::LinModel, transcription::TranscriptionMethod, nc::Int, - i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, i_Gmin, i_Gmax, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_Wmin, i_Wmax, i_x̂min, i_x̂max, args... ) -> i_b, i_g, A, Aeq, neq @@ -693,12 +693,12 @@ The argument `nc` is the number of custom nonlinear inequality constraints in 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 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_Gmin, A_Gmax, A_ŝ`. +`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, 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, i_Gmin, i_Gmax, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_Wmin, i_Wmax, i_x̂min, i_x̂max, args... ) where {NT<:Real} if isempty(args) @@ -708,21 +708,21 @@ function init_matconstraint_mpc( A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, - A_x̂min, A_x̂max, - A_Gmin, A_Gmax, + A_Wmin, A_Wmax, + 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; - A_Gmin; A_Gmax + A_Wmin; A_Wmax + 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_Gmin; i_Gmax] + i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Ymin; i_Ymax; i_Wmin; i_Wmax; i_x̂min; i_x̂max] i_g = trues(nc) return i_b, i_g, A, Aeq, neq end @@ -730,18 +730,18 @@ end "Init `i_b` without output & terminal constraints if `NonLinModel` and `SingleShooting`." function init_matconstraint_mpc( ::NonLinModel{NT}, ::SingleShooting, nc::Int, - i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, i_Gmin, i_Gmax, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_Wmin, i_Wmax, i_x̂min, i_x̂max, args... ) where {NT<:Real} if isempty(args) A, Aeq, neq = nothing, nothing, nothing else - A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , _ , _ , A_Gmin, A_Gmax, A_ŝ = args - A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Gmin; A_Gmax] + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, _ , _ , A_ŝ = args + A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Wmin; A_Wmax] Aeq = A_ŝ neq = 0 end - i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Gmin; i_Gmax] + i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Wmin; i_Wmax] i_g = [i_Ymin; i_Ymax; i_x̂min; i_x̂max; trues(nc)] return i_b, i_g, A, Aeq, neq end @@ -749,19 +749,19 @@ end "Init `i_b` without output constraints if `NonLinModel` and other `TranscriptionMethod`." function init_matconstraint_mpc( ::NonLinModel{NT}, ::TranscriptionMethod, nc::Int, - i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, i_Gmin, i_Gmax, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_Wmin, i_Wmax, i_x̂min, i_x̂max, args... ) where {NT<:Real} if isempty(args) A, Aeq, neq = nothing, nothing, nothing else - A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Gmin, A_Gmax, A_x̂min, A_x̂max, A_ŝ = args - A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_x̂min; A_x̂max; A_Gmin; A_Gmax] + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, A_x̂min, A_x̂max, A_ŝ = args + A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Wmin; A_Wmax; 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_Gmin; i_Gmax] + i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Wmin; i_Wmax; i_x̂min; i_x̂max] i_g = [i_Ymin; i_Ymax; trues(nc)] return i_b, i_g, A, Aeq, neq end @@ -773,12 +773,12 @@ Set `b` vector for the linear model inequality constraints (``\mathbf{A Z̃ ≤ Also init ``\mathbf{f_x̂} = \mathbf{g_x̂ d_0}(k) + \mathbf{j_x̂ D̂_0} + \mathbf{k_x̂ x̂_0}(k) + \mathbf{v_x̂ u_0}(k-1) + \mathbf{b_x̂}`` vector for the terminal constraints, see -[`init_predmat`](@ref). The ``\mathbf{F_G}`` vector for the custom linear constraints is -also updated, see [`relaxG`](@ref). +[`init_predmat`](@ref). The ``\mathbf{F_W}`` vector for the custom linear constraints is +also updated, see [`relaxW`](@ref). """ function linconstraint!(mpc::PredictiveController, model::LinModel, ::TranscriptionMethod) nU, nΔŨ, nY = length(mpc.con.U0min), length(mpc.con.ΔŨmin), length(mpc.con.Y0min) - nḠ = length(mpc.con.Gmin) + nW = length(mpc.con.Wmin) nx̂, fx̂ = mpc.estim.nx̂, mpc.con.fx̂ fx̂ .= mpc.con.bx̂ mul!(fx̂, mpc.con.kx̂, mpc.estim.x̂0, 1, 1) @@ -787,7 +787,7 @@ function linconstraint!(mpc::PredictiveController, model::LinModel, ::Transcript mul!(fx̂, mpc.con.gx̂, mpc.d0, 1, 1) mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1) end - mpc.con.nG > 0 && linconstraint_custom!(mpc, model) + mpc.con.nw > 0 && linconstraint_custom!(mpc, model) n = 0 mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0 n += nU @@ -801,13 +801,13 @@ function linconstraint!(mpc::PredictiveController, model::LinModel, ::Transcript n += nY mpc.con.b[(n+1):(n+nY)] .= @. +mpc.con.Y0max - mpc.F n += nY + mpc.con.b[(n+1):(n+nW)] .= @. -mpc.con.Wmin + mpc.con.FW + n += nW + mpc.con.b[(n+1):(n+nW)] .= @. +mpc.con.Wmax - mpc.con.FW + n += nW mpc.con.b[(n+1):(n+nx̂)] .= @. -mpc.con.x̂0min + fx̂ n += nx̂ mpc.con.b[(n+1):(n+nx̂)] .= @. +mpc.con.x̂0max - fx̂ - n += nx̂ - mpc.con.b[(n+1):(n+nḠ)] .= @. -mpc.con.Gmin + mpc.con.FG - n += nḠ - mpc.con.b[(n+1):(n+nḠ)] .= @. +mpc.con.Gmax - mpc.con.FG if any(mpc.con.i_b) lincon = mpc.optim[:linconstraint] JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b]) @@ -818,10 +818,10 @@ end "Set `b` excluding predicted output constraints for `NonLinModel` and not `SingleShooting`." function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::TranscriptionMethod) nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin) - nḠ = length(mpc.con.Gmin) + nW = length(mpc.con.Wmin) nx̂ = mpc.estim.nx̂ # here, updating fx̂ is not necessary since fx̂ = 0 - mpc.con.nG > 0 && linconstraint_custom!(mpc, model) + mpc.con.nw > 0 && linconstraint_custom!(mpc, model) n = 0 mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0 n += nU @@ -831,13 +831,13 @@ function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::Transcriptio n += nΔŨ mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax n += nΔŨ + mpc.con.b[(n+1):(n+nW)] .= @. -mpc.con.Wmin + mpc.con.FW + n += nW + mpc.con.b[(n+1):(n+nW)] .= @. +mpc.con.Wmax - mpc.con.FW + n += nW mpc.con.b[(n+1):(n+nx̂)] .= @. -mpc.con.x̂0min n += nx̂ mpc.con.b[(n+1):(n+nx̂)] .= @. +mpc.con.x̂0max - n += nx̂ - mpc.con.b[(n+1):(n+nḠ)] .= @. -mpc.con.Gmin + mpc.con.FG - n += nḠ - mpc.con.b[(n+1):(n+nḠ)] .= @. +mpc.con.Gmax - mpc.con.FG if any(mpc.con.i_b) lincon = mpc.optim[:linconstraint] JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b]) @@ -847,8 +847,8 @@ end "Also exclude terminal constraints for `NonLinModel` and `SingleShooting`." function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::SingleShooting) nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin) - nḠ = length(mpc.con.Gmin) - mpc.con.nG > 0 && linconstraint_custom!(mpc, model) + nW = length(mpc.con.Wmin) + mpc.con.nw > 0 && linconstraint_custom!(mpc, model) n = 0 mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0 n += nU @@ -858,9 +858,9 @@ function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::SingleShooti n += nΔŨ mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax n += nΔŨ - mpc.con.b[(n+1):(n+nḠ)] .= @. -mpc.con.Gmin + mpc.con.FG - n += nḠ - mpc.con.b[(n+1):(n+nḠ)] .= @. +mpc.con.Gmax - mpc.con.FG + mpc.con.b[(n+1):(n+nW)] .= @. -mpc.con.Gmin + mpc.con.FG + n += nW + mpc.con.b[(n+1):(n+nW)] .= @. +mpc.con.Gmax - mpc.con.FG if any(mpc.con.i_b) lincon = mpc.optim[:linconstraint] @views JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b]) @@ -868,34 +868,35 @@ function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::SingleShooti return nothing end -"Init the ``\\mathbf{F_G}`` vector for the linear model custom inequality constraints." +"Init the ``\\mathbf{F_W}`` vector for the linear model custom inequality constraints." function linconstraint_custom!(mpc::PredictiveController, model::SimModel) ny, nu, nd, buffer = model.ny, model.nu, model.nd, mpc.buffer - FG = mpc.con.FG + FW = mpc.con.FW Ue_term, D̂e_term, R̂e_term = buffer.Ue, buffer.D̂e, buffer.Ŷe - FG .= 0 + FW .= 0 Ue_term[1:end-nu] .= mpc.Tu_lastu0 .+ mpc.Uop Ue_term[end-nu+1:end] .= mpc.lastu0 .+ model.uop - mul!(FG, mpc.con.Ḡu, Ue_term, 1, 1) + mul!(FW, mpc.con.Ḡu, Ue_term, 1, 1) if model.nd > 0 D̂e_term[1:nd] .= mpc.d0 .+ model.dop D̂e_term[nd+1:end] .= mpc.D̂0 .+ model.D̂op - mul!(FG, mpc.con.Ḡd, D̂e_term, 1, 1) + mul!(FW, mpc.con.Ḡd, D̂e_term, 1, 1) end R̂e_term[1:ny] .= mpc.ry R̂e_term[ny+1:end] .= mpc.R̂y - mul!(FG, mpc.con.Ḡr, R̂e_term, 1, 1) + mul!(FW, mpc.con.Ḡr, R̂e_term, 1, 1) return linconstraint_custom_outputs!(mpc, model) end "Also include the `Ḡy` term in the custom linear constraints for [`LinModel`](@ref)." function linconstraint_custom_outputs!(mpc::PredictiveController, model::LinModel) - Ŷe_term, FG, ny = mpc.buffer.Ŷe, mpc.con.FG, model.ny + Ŷe_term, FW, ny = mpc.buffer.Ŷe, mpc.con.FW, model.ny Ŷe_term[1:ny] .= mpc.ŷ Ŷe_term[ny+1:end] .= mpc.F .+ mpc.Yop - mul!(FG, mpc.con.Ḡy, Ŷe_term, 1, 1) + mul!(FW, mpc.con.Ḡy, Ŷe_term, 1, 1) return nothing end +"Do nothing for other model types." linconstraint_custom_outputs!(::PredictiveController, ::SimModel) = nothing From 55e30560a623b7bf7fae71641d19b25a21182365 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 12:03:33 -0500 Subject: [PATCH 16/43] changed: renamming some forgotten symbols --- docs/src/internals/predictive_control.md | 2 +- src/controller/construct.jl | 16 ++++++++-------- src/controller/nonlinmpc.jl | 10 +++++----- src/controller/transcription.jl | 14 +++++++------- 4 files changed, 21 insertions(+), 21 deletions(-) diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index d5f3823b6..ef79ef649 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -20,8 +20,8 @@ ModelPredictiveControl.init_defectmat ModelPredictiveControl.relaxU ModelPredictiveControl.relaxΔU ModelPredictiveControl.relaxŶ +ModelPredictiveControl.relaxW ModelPredictiveControl.relaxterminal -ModelPredictiveControl.relaxG ModelPredictiveControl.augmentdefect ModelPredictiveControl.init_quadprog ModelPredictiveControl.init_stochpred diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 364940cab..04f72dd1f 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -143,12 +143,12 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} # custom linear equality constraints: ẼW ::Matrix{NT} FW ::Vector{NT} - Ḡy ::SparseMatrixCSC{NT, Int} - Ḡu ::SparseMatrixCSC{NT, Int} - Ḡd ::SparseMatrixCSC{NT, Int} - Ḡr ::SparseMatrixCSC{NT, Int} + W̄y ::SparseMatrixCSC{NT, Int} + W̄u ::SparseMatrixCSC{NT, Int} + W̄d ::SparseMatrixCSC{NT, Int} + W̄r ::SparseMatrixCSC{NT, Int} nw ::Int - # bounds over the prediction horizon (deviation vectors from operating points): + # bounds over the prediction horizon (deviation vectors from operating points): U0min ::Vector{NT} U0max ::Vector{NT} ΔŨmin ::Vector{NT} @@ -472,12 +472,12 @@ function setconstraint!( if !isnothing(C_wmin) size(C_wmin) == (nw*(Hp+1),) || throw(ArgumentError("C_wmin size must be $((nw*(Hp+1),))")) any(<(0), C_wmin) && error("C_wmin weights should be non-negative") - con.A_Gmin[:, end] .= -C_wmin + con.A_Wmin[:, end] .= -C_wmin end if !isnothing(C_wmax) size(C_wmax) == (nw*(Hp+1),) || throw(ArgumentError("C_wmax size must be $((nw*(Hp+1),))")) any(<(0), C_wmax) && error("C_wmax weights should be non-negative") - con.A_Gmax[:, end] .= -C_wmax + con.A_Wmax[:, end] .= -C_wmax end if !isnothing(c_x̂min) size(c_x̂min) == (nx̂,) || throw(ArgumentError("c_x̂min size must be $((nx̂,))")) @@ -811,7 +811,7 @@ function init_defaultcon_mpc( A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂max, A_x̂min, A_ŝ ) - # dummy fx̂, FG and Fŝ vectors (updated just before optimization) + # dummy fx̂, FW and Fŝ vectors (updated just before optimization) fx̂, FW, Fŝ = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nx̂*Hp) # dummy b and beq vectors (updated just before optimization) b, beq = zeros(NT, size(A, 1)), zeros(NT, size(Aeq, 1)) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index f6e69d6fb..35d39e69a 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -210,10 +210,10 @@ This controller allocates memory at each time step for the optimization. - `N_Hc=Diagonal(repeat(Nwt,Hc))` : positive semidefinite symmetric matrix ``\mathbf{N}_{H_c}``. - `L_Hp=Diagonal(repeat(Lwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{L}_{H_p}``. - `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only. -- `Gy=nothing` : custom linear constraint matrix for output (see Extended Help). -- `Gu=nothing` : custom linear constraint matrix for manipulated input (see Extended Help). -- `Gd=nothing` : custom linear constraint matrix for meas. disturbance (see Extended Help). -- `Gr=nothing` : custom linear constraint matrix for output setpoint (see Extended Help). +- `Wy=nothing` : custom linear constraint matrix for output (see Extended Help). +- `Wu=nothing` : custom linear constraint matrix for manipulated input (see Extended Help). +- `Wd=nothing` : custom linear constraint matrix for meas. disturbance (see Extended Help). +- `Wr=nothing` : custom linear constraint matrix for output setpoint (see Extended Help). - `Ewt=0.0` : economic costs weight ``E`` (scalar). - `JE=(_,_,_,_)->0.0` : economic or custom cost function ``J_E(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p})``. @@ -267,7 +267,7 @@ NonLinMPC controller with a sample time Ts = 10.0 s: algebra instead of a `for` loop. This feature can accelerate the optimization, especially for the constraint handling, and is not available in any other package, to my knowledge. See [`setconstraint!`](@ref) for details about the custom linear inequality constraint - matrices `Gy`, `Gu`, `Gd` and `Gr`. The `Gy` keyword argument can be provided only if + matrices `Wy`, `Wu`, `Wd` and `Wr`. The `Wy` keyword argument can be provided only if `model` is a [`LinModel`](@ref)). The economic cost ``J_E`` and custom constraint ``\mathbf{g_c}`` functions receive the diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index be406f5ad..a656176e8 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -858,9 +858,9 @@ function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::SingleShooti n += nΔŨ mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax n += nΔŨ - mpc.con.b[(n+1):(n+nW)] .= @. -mpc.con.Gmin + mpc.con.FG + mpc.con.b[(n+1):(n+nW)] .= @. -mpc.con.Wmin + mpc.con.FW n += nW - mpc.con.b[(n+1):(n+nW)] .= @. +mpc.con.Gmax - mpc.con.FG + mpc.con.b[(n+1):(n+nW)] .= @. +mpc.con.Wmax - mpc.con.FW if any(mpc.con.i_b) lincon = mpc.optim[:linconstraint] @views JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b]) @@ -876,24 +876,24 @@ function linconstraint_custom!(mpc::PredictiveController, model::SimModel) FW .= 0 Ue_term[1:end-nu] .= mpc.Tu_lastu0 .+ mpc.Uop Ue_term[end-nu+1:end] .= mpc.lastu0 .+ model.uop - mul!(FW, mpc.con.Ḡu, Ue_term, 1, 1) + mul!(FW, mpc.con.W̄u, Ue_term, 1, 1) if model.nd > 0 D̂e_term[1:nd] .= mpc.d0 .+ model.dop D̂e_term[nd+1:end] .= mpc.D̂0 .+ model.D̂op - mul!(FW, mpc.con.Ḡd, D̂e_term, 1, 1) + mul!(FW, mpc.con.W̄d, D̂e_term, 1, 1) end R̂e_term[1:ny] .= mpc.ry R̂e_term[ny+1:end] .= mpc.R̂y - mul!(FW, mpc.con.Ḡr, R̂e_term, 1, 1) + mul!(FW, mpc.con.W̄r, R̂e_term, 1, 1) return linconstraint_custom_outputs!(mpc, model) end -"Also include the `Ḡy` term in the custom linear constraints for [`LinModel`](@ref)." +"Also include the `W̄y` term in the custom linear constraints for [`LinModel`](@ref)." function linconstraint_custom_outputs!(mpc::PredictiveController, model::LinModel) Ŷe_term, FW, ny = mpc.buffer.Ŷe, mpc.con.FW, model.ny Ŷe_term[1:ny] .= mpc.ŷ Ŷe_term[ny+1:end] .= mpc.F .+ mpc.Yop - mul!(FW, mpc.con.Ḡy, Ŷe_term, 1, 1) + mul!(FW, mpc.con.W̄y, Ŷe_term, 1, 1) return nothing end "Do nothing for other model types." From 8df9509c384948aa3490ee34c600909cc5d19fb9 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 13:20:36 -0500 Subject: [PATCH 17/43] doc: details on custom linear const. softness weights. --- src/controller/construct.jl | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 04f72dd1f..99dcc3525 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -211,10 +211,10 @@ The predictive controllers support both soft and hard constraints, defined by: ``` and also ``ϵ ≥ 0``. The last line is the terminal constraints applied on the states at the end of the horizon (see Extended Help). See [`MovingHorizonEstimator`](@ref) constraints -for details on bounds and softness parameters ``\mathbf{c}``. The penultimate line with -``\mathbf{w}`` vector is for the custom linear inequality constraints. See Extended Help for -details on custom linear and time-varying constraints. The output, custom linear and -terminal constraints are all soft by default. +for details on bounds and softness parameters ``\mathbf{c}``. The penultimate line with the +``\mathbf{w}`` vector is for custom linear inequality constraints. See Extended Help for +details on this and time-varying bounds. The output, custom and terminal constraints are all +soft by default. # Arguments !!! info @@ -234,6 +234,7 @@ terminal constraints are all soft by default. - `c_umin=fill(0.0,nu)` / `c_umax=fill(0.0,nu)` : `umin` / `umax` softness weight ``\mathbf{c_{u_{min/max}}}`` - `c_Δumin=fill(0.0,nu)` / `c_Δumax=fill(0.0,nu)` : `Δumin` / `Δumax` softness weight ``\mathbf{c_{Δu_{min/max}}}`` - `c_ymin=fill(1.0,ny)` / `c_ymax=fill(1.0,ny)` : `ymin` / `ymax` softness weight ``\mathbf{c_{y_{min/max}}}`` +- `c_wmin=fill(1.0,nw)` / `c_wmax=fill(1.0,nw)` : `wmin` / `wmax` softness weight ``\mathbf{c_{w_{min/max}}}`` - `c_x̂min=fill(1.0,nx̂)` / `c_x̂max=fill(1.0,nx̂)` : `x̂min` / `x̂max` softness weight ``\mathbf{c_{x̂_{min/max}}}`` - all the keyword arguments above but with a first capital letter, except for the terminal constraints, e.g. `Ymax` or `C_Δumin`: for time-varying constraints (see Extended Help) @@ -295,13 +296,18 @@ LinMPC controller with a sample time Ts = 4.0 s: - `Ymin` / `Ymax` / `C_ymin` / `C_ymax` : ``\mathbf{Ŷ}`` constraints `(ny*Hp,)`. - `Wmin` / `Wmax` / `C_wmin` / `C_wmax` : custom linear constraints `(nw*(Hp+1),)`. - The custom constraints are all gathered in the vector: - ```math - \mathbf{W} = \begin{bmatrix} + The custom linear inequality constraints are all gathered in the vector: + ```math + \begin{aligned} + \mathbf{W} + &= \begin{bmatrix} + \mathbf{w}(k+0) \\ \mathbf{w}(k+1) \\ \vdots \\ \mathbf{w}(k+H_p) \end{bmatrix} \\ + &= \begin{bmatrix} \mathbf{W_y ŷ}(k+0) + \mathbf{W_u u}(k+0) + \mathbf{W_d d}(k+0) + \mathbf{W_r r_y}(k+0) \\ \mathbf{W_y ŷ}(k+1) + \mathbf{W_u u}(k+1) + \mathbf{W_d d̂}(k+1) + \mathbf{W_r r̂_y}(k+1) \\ \vdots \\ \mathbf{W_y ŷ}(k+H_p) + \mathbf{W_u u}(k+H_p) + \mathbf{W_d d̂}(k+H_p) + \mathbf{W_r r̂_y}(k+H_p) \end{bmatrix} + \end{aligned} ``` The matrices ``\mathbf{W_y}``, ``\mathbf{W_u}``, ``\mathbf{W_d}`` and ``\mathbf{W_r}`` must have `nw` rows and are provided at construction time. The terms with From d6013691eec78b38acc54df4d766b85b2a8c6846 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 13:35:27 -0500 Subject: [PATCH 18/43] changed: renamed EW->Ew and FW->Fw --- src/controller/construct.jl | 26 +++++++++++++------------- src/controller/transcription.jl | 26 +++++++++++++------------- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 99dcc3525..090dfc27f 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -141,8 +141,8 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} Vŝ ::Matrix{NT} Bŝ ::Vector{NT} # custom linear equality constraints: - ẼW ::Matrix{NT} - FW ::Vector{NT} + Ẽw ::Matrix{NT} + Fw ::Vector{NT} W̄y ::SparseMatrixCSC{NT, Int} W̄u ::SparseMatrixCSC{NT, Int} W̄d ::SparseMatrixCSC{NT, Int} @@ -803,7 +803,7 @@ function init_defaultcon_mpc( A_Umin, A_Umax, P̃u = relaxU(Pu, C_umin, C_umax, nϵ) A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃Δu = relaxΔU(PΔu, C_Δumin, C_Δumax, ΔUmin, ΔUmax, nϵ) A_Ymin, A_Ymax, Ẽ = relaxŶ(E, C_ymin, C_ymax, nϵ) - A_Wmin, A_Wmax, ẼW = relaxW(E, Pu, Hp, W̄y, W̄u, C_wmin, C_wmax, nϵ) + A_Wmin, A_Wmax, Ẽw = relaxW(E, Pu, Hp, W̄y, W̄u, C_wmin, C_wmax, nϵ) A_x̂min, A_x̂max, ẽx̂ = relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ) A_ŝ, Ẽŝ = augmentdefect(Eŝ, nϵ) i_Umin, i_Umax = .!isinf.(U0min), .!isinf.(U0max) @@ -817,14 +817,14 @@ function init_defaultcon_mpc( A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂max, A_x̂min, A_ŝ ) - # dummy fx̂, FW and Fŝ vectors (updated just before optimization) - fx̂, FW, Fŝ = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nx̂*Hp) + # dummy fx̂, Fw and Fŝ vectors (updated just before optimization) + fx̂, Fw, Fŝ = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nx̂*Hp) # dummy b and beq vectors (updated just before optimization) b, beq = zeros(NT, size(A, 1)), zeros(NT, size(Aeq, 1)) con = ControllerConstraint{NT, GCfunc}( ẽx̂ , fx̂ , gx̂ , jx̂ , kx̂ , vx̂ , bx̂ , Ẽŝ , Fŝ , Gŝ , Jŝ , Kŝ , Vŝ , Bŝ , - ẼW , FW , W̄y , W̄u , W̄d , W̄r , nw , + Ẽw , Fw , W̄y , W̄u , W̄d , W̄r , nw , U0min , U0max , ΔŨmin , ΔŨmax , Y0min , Y0max , Wmin , Wmax , x̂0min , x̂0max , A_Umin , A_Umax , A_ΔŨmin , A_ΔŨmax , @@ -973,7 +973,7 @@ function relaxŶ(E::AbstractMatrix{NT}, C_ymin, C_ymax, nϵ) where NT<:Real end @doc raw""" - relaxW(E, Pu, nw, W̄y, W̄u, C_wmin, C_wmax, nϵ) -> A_Wmin, A_Wmax, ẼW + relaxW(E, Pu, nw, W̄y, W̄u, C_wmin, C_wmax, nϵ) -> A_Wmin, A_Wmax, Ẽw Construct and augment the custom linear constraints with slack variable ϵ for softening. @@ -1036,17 +1036,17 @@ function relaxW(E::AbstractMatrix{NT}, Pu, Hp, W̄y, W̄u, C_wmin, C_wmax, nϵ) Wy_terms = W̄y*[zeros(NT, ny, size(E, 2)); E] end Wu_terms = W̄u*[Pu; Pu[end-nu+1:end, :]] - EW = Wy_terms + Wu_terms + Ew = Wy_terms + Wu_terms if nϵ == 1 # Z̃ = [Z; ϵ] # ϵ impacts custom constraint calculations: - A_Wmin, A_Wmax = -[EW C_wmin], [EW -C_wmax] + A_Wmin, A_Wmax = -[Ew C_wmin], [Ew -C_wmax] # ϵ has no impact on custom constraint predictions: - ẼW = [EW zeros(NT, nW, 1)] + Ẽw = [Ew zeros(NT, nW, 1)] else # Z̃ = Z (only hard constraints) - ẼW = EW - A_Wmin, A_Wmax = -EW, EW + Ẽw = Ew + A_Wmin, A_Wmax = -Ew, Ew end - return A_Wmin, A_Wmax, ẼW + return A_Wmin, A_Wmax, Ẽw end @doc raw""" diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index a656176e8..4e14bc16a 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -801,9 +801,9 @@ function linconstraint!(mpc::PredictiveController, model::LinModel, ::Transcript n += nY mpc.con.b[(n+1):(n+nY)] .= @. +mpc.con.Y0max - mpc.F n += nY - mpc.con.b[(n+1):(n+nW)] .= @. -mpc.con.Wmin + mpc.con.FW + mpc.con.b[(n+1):(n+nW)] .= @. -mpc.con.Wmin + mpc.con.Fw n += nW - mpc.con.b[(n+1):(n+nW)] .= @. +mpc.con.Wmax - mpc.con.FW + mpc.con.b[(n+1):(n+nW)] .= @. +mpc.con.Wmax - mpc.con.Fw n += nW mpc.con.b[(n+1):(n+nx̂)] .= @. -mpc.con.x̂0min + fx̂ n += nx̂ @@ -831,9 +831,9 @@ function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::Transcriptio n += nΔŨ mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax n += nΔŨ - mpc.con.b[(n+1):(n+nW)] .= @. -mpc.con.Wmin + mpc.con.FW + mpc.con.b[(n+1):(n+nW)] .= @. -mpc.con.Wmin + mpc.con.Fw n += nW - mpc.con.b[(n+1):(n+nW)] .= @. +mpc.con.Wmax - mpc.con.FW + mpc.con.b[(n+1):(n+nW)] .= @. +mpc.con.Wmax - mpc.con.Fw n += nW mpc.con.b[(n+1):(n+nx̂)] .= @. -mpc.con.x̂0min n += nx̂ @@ -858,9 +858,9 @@ function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::SingleShooti n += nΔŨ mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax n += nΔŨ - mpc.con.b[(n+1):(n+nW)] .= @. -mpc.con.Wmin + mpc.con.FW + mpc.con.b[(n+1):(n+nW)] .= @. -mpc.con.Wmin + mpc.con.Fw n += nW - mpc.con.b[(n+1):(n+nW)] .= @. +mpc.con.Wmax - mpc.con.FW + mpc.con.b[(n+1):(n+nW)] .= @. +mpc.con.Wmax - mpc.con.Fw if any(mpc.con.i_b) lincon = mpc.optim[:linconstraint] @views JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b]) @@ -871,29 +871,29 @@ end "Init the ``\\mathbf{F_W}`` vector for the linear model custom inequality constraints." function linconstraint_custom!(mpc::PredictiveController, model::SimModel) ny, nu, nd, buffer = model.ny, model.nu, model.nd, mpc.buffer - FW = mpc.con.FW + Fw = mpc.con.Fw Ue_term, D̂e_term, R̂e_term = buffer.Ue, buffer.D̂e, buffer.Ŷe - FW .= 0 + Fw .= 0 Ue_term[1:end-nu] .= mpc.Tu_lastu0 .+ mpc.Uop Ue_term[end-nu+1:end] .= mpc.lastu0 .+ model.uop - mul!(FW, mpc.con.W̄u, Ue_term, 1, 1) + mul!(Fw, mpc.con.W̄u, Ue_term, 1, 1) if model.nd > 0 D̂e_term[1:nd] .= mpc.d0 .+ model.dop D̂e_term[nd+1:end] .= mpc.D̂0 .+ model.D̂op - mul!(FW, mpc.con.W̄d, D̂e_term, 1, 1) + mul!(Fw, mpc.con.W̄d, D̂e_term, 1, 1) end R̂e_term[1:ny] .= mpc.ry R̂e_term[ny+1:end] .= mpc.R̂y - mul!(FW, mpc.con.W̄r, R̂e_term, 1, 1) + mul!(Fw, mpc.con.W̄r, R̂e_term, 1, 1) return linconstraint_custom_outputs!(mpc, model) end "Also include the `W̄y` term in the custom linear constraints for [`LinModel`](@ref)." function linconstraint_custom_outputs!(mpc::PredictiveController, model::LinModel) - Ŷe_term, FW, ny = mpc.buffer.Ŷe, mpc.con.FW, model.ny + Ŷe_term, Fw, ny = mpc.buffer.Ŷe, mpc.con.Fw, model.ny Ŷe_term[1:ny] .= mpc.ŷ Ŷe_term[ny+1:end] .= mpc.F .+ mpc.Yop - mul!(FW, mpc.con.W̄y, Ŷe_term, 1, 1) + mul!(Fw, mpc.con.W̄y, Ŷe_term, 1, 1) return nothing end "Do nothing for other model types." From 10b9963ba3fe7537f89cfc657bd822735c2ca158 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 13:57:32 -0500 Subject: [PATCH 19/43] added: `setmodel! ` support for custom linear constraints --- src/controller/execute.jl | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index c1e7befe7..b1e6b6e31 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -629,11 +629,14 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) weights = mpc.weights nu, ny, nd, Hp, Hc, nb = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc, mpc.nb optim, con = mpc.optim, mpc.con + nZ = get_nZ(estim, transcription, Hp, Hc) + Pu = mpc.P̃u[:, 1:nZ] # --- prediction matrices --- E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( model, estim, transcription, Hp, Hc, nb ) A_Ymin, A_Ymax, Ẽ = relaxŶ(E, con.C_ymin, con.C_ymax, mpc.nϵ) + A_Wmin, A_Wmax, Ẽw = relaxW(E, Pu, Hp, con.W̄y, con.W̄u, con.C_wmin, con.C_wmax, mpc.nϵ) A_x̂min, A_x̂max, ẽx̂ = relaxterminal(ex̂, con.c_x̂min, con.c_x̂max, mpc.nϵ) mpc.Ẽ .= Ẽ mpc.G .= G @@ -641,6 +644,13 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) mpc.K .= K mpc.V .= V mpc.B .= B + # --- terminal constraints --- + con.ẽx̂ .= ẽx̂ + con.gx̂ .= gx̂ + con.jx̂ .= jx̂ + con.kx̂ .= kx̂ + con.vx̂ .= vx̂ + con.bx̂ .= bx̂ # --- defect matrices --- Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) A_ŝ, Ẽŝ = augmentdefect(Eŝ, mpc.nϵ) @@ -650,15 +660,13 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) con.Kŝ .= Kŝ con.Vŝ .= Vŝ con.Bŝ .= Bŝ + # --- custom linear constraints --- + con.Ẽw .= Ẽw # --- linear inequality constraints --- - con.ẽx̂ .= ẽx̂ - con.gx̂ .= gx̂ - con.jx̂ .= jx̂ - con.kx̂ .= kx̂ - con.vx̂ .= vx̂ - con.bx̂ .= bx̂ con.A_Ymin .= A_Ymin con.A_Ymax .= A_Ymax + con.A_Wmin .= A_Wmin + con.A_Wmax .= A_Wmax con.A_x̂min .= A_x̂min con.A_x̂max .= A_x̂max con.A .= [ From d217df2ef04024ec1d4a6b35c53b26add80a160c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 14:00:58 -0500 Subject: [PATCH 20/43] added: error for conversion to `LinearMPC.MPC` It is not supported for now, I will add the support in a following PR. --- ext/LinearMPCext.jl | 1 + 1 file changed, 1 insertion(+) diff --git a/ext/LinearMPCext.jl b/ext/LinearMPCext.jl index b4db22980..595df0be9 100644 --- a/ext/LinearMPCext.jl +++ b/ext/LinearMPCext.jl @@ -180,6 +180,7 @@ end function validate_constraints(mpc::ModelPredictiveControl.LinMPC) nΔU = mpc.Hc * mpc.estim.model.nu + mpc.con.nw > 0 && error("Conversion of custom linear inequality constraints is not supported for now.") mpc.weights.isinf_C && return nothing # only hard constraints are entirely supported C_umin, C_umax = -mpc.con.A_Umin[:, end], -mpc.con.A_Umax[:, end] C_Δumin, C_Δumax = -mpc.con.A_ΔŨmin[1:nΔU, end], -mpc.con.A_ΔŨmax[1:nΔU, end] From b87fd440375f21aea721e676f4fe1f74b153a950 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 14:51:43 -0500 Subject: [PATCH 21/43] debug: also store `C_wmin` and `C_wmax` --- src/controller/construct.jl | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 090dfc27f..50508496f 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -182,9 +182,11 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} beq ::Vector{NT} # nonlinear equality constraints: neq ::Int - # constraint softness parameter vectors for the nonlinear inequality constraints: + # constraint softness parameter vectors needing seperate storage: C_ymin ::Vector{NT} C_ymax ::Vector{NT} + C_wmin ::Vector{NT} + C_wmax ::Vector{NT} c_x̂min ::Vector{NT} c_x̂max ::Vector{NT} # indices of finite numbers in the g vector (nonlinear inequality constraints): @@ -478,11 +480,13 @@ function setconstraint!( if !isnothing(C_wmin) size(C_wmin) == (nw*(Hp+1),) || throw(ArgumentError("C_wmin size must be $((nw*(Hp+1),))")) any(<(0), C_wmin) && error("C_wmin weights should be non-negative") + con.C_wmin .= C_wmin con.A_Wmin[:, end] .= -C_wmin end if !isnothing(C_wmax) size(C_wmax) == (nw*(Hp+1),) || throw(ArgumentError("C_wmax size must be $((nw*(Hp+1),))")) any(<(0), C_wmax) && error("C_wmax weights should be non-negative") + con.C_wmax .= C_wmax con.A_Wmax[:, end] .= -C_wmax end if !isnothing(c_x̂min) @@ -833,7 +837,7 @@ function init_defaultcon_mpc( A_ŝ , Aeq , beq , neq , - C_ymin , C_ymax , c_x̂min , c_x̂max , + C_ymin , C_ymax , C_wmin , C_wmax , c_x̂min , c_x̂max , i_g , gc! , nc ) From 26064dc5dcc1d5ac7bcee0b82b9cffe68b27da88 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 15:07:25 -0500 Subject: [PATCH 22/43] test: `LinMPC` construction with custom linear constraints --- src/controller/construct.jl | 10 +++++----- test/3_test_predictive_control.jl | 30 ++++++++++++++++++++++++++---- 2 files changed, 31 insertions(+), 9 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 50508496f..e71df6b98 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -108,18 +108,18 @@ function validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, C=Inf, E=nothing) Hp < 1 && throw(ArgumentError("Prediction horizon Hp should be ≥ 1")) Hc < 1 && throw(ArgumentError("Control horizon Hc should be ≥ 1")) Hc > Hp && throw(ArgumentError("Control horizon Hc should be ≤ prediction horizon Hp")) - size(M_Hp) ≠ (nM,nM) && throw(ArgumentError("M_Hp size $(size(M_Hp)) ≠ (ny*Hp, ny*Hp) ($nM,$nM)")) - size(N_Hc) ≠ (nN,nN) && throw(ArgumentError("N_Hc size $(size(N_Hc)) ≠ (nu*Hc, nu*Hc) ($nN,$nN)")) - size(L_Hp) ≠ (nL,nL) && throw(ArgumentError("L_Hp size $(size(L_Hp)) ≠ (nu*Hp, nu*Hp) ($nL,$nL)")) + size(M_Hp) ≠ (nM,nM) && throw(DimensionMismatch("M_Hp size $(size(M_Hp)) ≠ (ny*Hp, ny*Hp) ($nM,$nM)")) + size(N_Hc) ≠ (nN,nN) && throw(DimensionMismatch("N_Hc size $(size(N_Hc)) ≠ (nu*Hc, nu*Hc) ($nN,$nN)")) + size(L_Hp) ≠ (nL,nL) && throw(DimensionMismatch("L_Hp size $(size(L_Hp)) ≠ (nu*Hp, nu*Hp) ($nL,$nL)")) (isdiag(M_Hp) && any(diag(M_Hp) .< 0)) && throw(ArgumentError("Mwt values should be nonnegative")) (isdiag(N_Hc) && any(diag(N_Hc) .< 0)) && throw(ArgumentError("Nwt values should be nonnegative")) (isdiag(L_Hp) && any(diag(L_Hp) .< 0)) && throw(ArgumentError("Lwt values should be nonnegative")) !ishermitian(M_Hp) && throw(ArgumentError("M_Hp should be hermitian")) !ishermitian(N_Hc) && throw(ArgumentError("N_Hc should be hermitian")) !ishermitian(L_Hp) && throw(ArgumentError("L_Hp should be hermitian")) - size(C) ≠ () && throw(ArgumentError("Cwt should be a real scalar")) + size(C) ≠ () && throw(DimensionMismatch("Cwt should be a real scalar")) C < 0 && throw(ArgumentError("Cwt weight should be ≥ 0")) - !isnothing(E) && size(E) ≠ () && throw(ArgumentError("Ewt should be a real scalar")) + !isnothing(E) && size(E) ≠ () && throw(DimensionMismatch("Ewt should be a real scalar")) end "Include all the data for the constraints of [`PredictiveController`](@ref)" diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 0568a8f02..7de68fdef 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -53,6 +53,23 @@ mpc16 = LinMPC(model, Hc=[1,2,3,6,6,6], Hp=10, Cwt=Inf) @test mpc16.Hc == 4 # the last 2 elements of Hc are ignored @test size(mpc16.P̃u) == (10*mpc1.estim.model.nu, 4*mpc1.estim.model.nu) + + Wy = ones(3, ny) + mpc17 = LinMPC(model; Wy) + @test mpc17.con.W̄y ≈ ModelPredictiveControl.repeatdiag(Wy, mpc17.Hp+1) + @test mpc17.con.W̄u ≈ zeros(model.nu*(mpc17.Hp+1), model.nu*(mpc17.Hp+1)) + @test mpc17.con.W̄d ≈ zeros(model.nd*(mpc17.Hp+1), model.nd*(mpc17.Hp+1)) + @test mpc17.con.W̄r ≈ zeros(model.ny*(mpc17.Hp+1), model.ny*(mpc17.Hp+1)) + + Wy = ones(2, model.ny) + Wu = 2*ones(2, model.nu) + Wd = 3*ones(2, model.nd) + Wr = 0.5*ones(2, model.ny) + mpc18 = LinMPC(model; Wy, Wu, Wd, Wr) + @test mpc18.con.W̄y ≈ ModelPredictiveControl.repeatdiag(Wy, mpc18.Hp+1) + @test mpc18.con.W̄u ≈ ModelPredictiveControl.repeatdiag(Wu, mpc18.Hp+1) + @test mpc18.con.W̄d ≈ ModelPredictiveControl.repeatdiag(Wd, mpc18.Hp+1) + @test mpc18.con.W̄r ≈ ModelPredictiveControl.repeatdiag(Wr, mpc18.Hp+1) @test_logs( (:warn, @@ -62,14 +79,19 @@ ) @test_throws ArgumentError LinMPC(model, Hc=0) @test_throws ArgumentError LinMPC(model, Hp=1, Hc=2) - @test_throws ArgumentError LinMPC(model, Mwt=[1]) - @test_throws ArgumentError LinMPC(model, Mwt=[1]) - @test_throws ArgumentError LinMPC(model, Lwt=[1]) - @test_throws ArgumentError LinMPC(model, Cwt=[1]) + @test_throws DimensionMismatch LinMPC(model, Mwt=[1]) + @test_throws DimensionMismatch LinMPC(model, Mwt=[1]) + @test_throws DimensionMismatch LinMPC(model, Lwt=[1]) + @test_throws DimensionMismatch LinMPC(model, Cwt=[1]) @test_throws ArgumentError LinMPC(model, Mwt=[-1,1]) @test_throws ArgumentError LinMPC(model, Nwt=[-1,1]) @test_throws ArgumentError LinMPC(model, Lwt=[-1,1]) @test_throws ArgumentError LinMPC(model, Cwt=-1) + @test_throws DimensionMismatch LinMPC(model, Wy=ones(2, ny+1)) + @test_throws DimensionMismatch LinMPC(model, Wu=ones(2, nu-1)) + @test_throws DimensionMismatch LinMPC(model, Wd=ones(2, nd+1)) + @test_throws DimensionMismatch LinMPC(model, Wr=ones(2, ny-1)) + @test_throws DimensionMismatch LinMPC(model, Wy=ones(2, ny), Wu=ones(3, nu)) end @testitem "LinMPC moves and getinfo" setup=[SetupMPCtests] begin From 0f7aff7fe5e8d9aae9044e3afc548452ad2421aa Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 15:11:47 -0500 Subject: [PATCH 23/43] doc: minor comment in `ExplicitMPC` --- src/controller/explicitmpc.jl | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index b786e5edf..414155724 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -106,8 +106,9 @@ The controller minimizes the following objective function at each discrete time See [`LinMPC`](@ref) for the variable definitions. This controller does not support constraints but the computational costs are extremely low (array division), therefore suitable for applications that require small sample times. The keyword arguments are -identical to [`LinMPC`](@ref), except for `Cwt`, `transcription` and `optim`, which are not -supported. It uses a [`SingleShooting`](@ref) transcription method and is allocation-free. +identical to [`LinMPC`](@ref), except for `Cwt`, `Wy`, `Wu`, `Wd`, `Wr`, `transcription` and +`optim`, which are not supported. It uses a [`SingleShooting`](@ref) transcription method +and is allocation-free. This method uses the default state estimator, a [`SteadyKalmanFilter`](@ref) with default arguments. From 03d1106d650956bb345b246442bdc1beccc39a0b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 15:45:43 -0500 Subject: [PATCH 24/43] doc: using small case `w` in equations --- src/controller/construct.jl | 16 ++++++++-------- src/controller/transcription.jl | 6 +++--- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index e71df6b98..276ed0487 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -993,11 +993,11 @@ By introducing the following block-diagonal matrices with ``H_p + 1`` blocks: the ``\mathbf{W}`` vector defined in the Extended Help section of [`setconstraint!`](@ref) can be expressed as: ```math -\mathbf{W} = \mathbf{E_W} \mathbf{Z} + \mathbf{F_W} +\mathbf{W} = \mathbf{E_w} \mathbf{Z} + \mathbf{F_w} ``` in which: ```math -\mathbf{E_W} = \mathbf{W̄_y} [\begin{smallmatrix} \mathbf{0} \\ \mathbf{E} \end{smallmatrix}] + +\mathbf{E_w} = \mathbf{W̄_y} [\begin{smallmatrix} \mathbf{0} \\ \mathbf{E} \end{smallmatrix}] + \mathbf{W̄_u} [\begin{smallmatrix} \mathbf{P_u} \\ \mathbf{p_u} \end{smallmatrix}] ``` The ``\mathbf{E}`` matrix appears in the linear output prediction equation @@ -1005,10 +1005,10 @@ The ``\mathbf{E}`` matrix appears in the linear output prediction equation ``\mathbf{U = P_u Z + T_u u}(k-1)``. The ``\mathbf{p_u}`` matrix corresponds to the last `nu` rows of ``\mathbf{P_u}``. The ``\mathbf{W̄_u}`` term assumes that ``H_c ≤ H_p``, hence ``\mathbf{Δu}(k + H_c) = \mathbf{0}`` and ``\mathbf{u}(k + H_c) = \mathbf{u}(k + H_c - 1)``. -The ``\mathbf{F_W}`` vector is updated at each control period `k` in [`linconstraint!`](@ref) +The ``\mathbf{F_w}`` vector is updated at each control period `k` in [`linconstraint!`](@ref) method, and is defined as: ```math -\mathbf{F_W} = +\mathbf{F_w} = \mathbf{W̄_y} [\begin{smallmatrix} \mathbf{ŷ}(k) \\ \mathbf{F + Y_{op}} \end{smallmatrix}] + \mathbf{W̄_u} [\begin{smallmatrix} \mathbf{T_u u}(k-1) \\ \mathbf{u}(k-1) \end{smallmatrix}] + \mathbf{W̄_d} [\begin{smallmatrix} \mathbf{d}(k) \\ \mathbf{D̂} \end{smallmatrix}] + @@ -1016,16 +1016,16 @@ method, and is defined as: ``` Denoting the decision variables augmented with the slack variable ``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, the function -returns the ``\mathbf{Ẽ_W}`` matrix that appears in the custom constraint equation -``\mathbf{W = Ẽ_W Z̃ + F_W}``, and the ``\mathbf{A}`` matrices for the inequality constraints: +returns the ``\mathbf{Ẽ_w}`` matrix that appears in the custom constraint equation +``\mathbf{W = Ẽ_w Z̃ + F_w}``, and the ``\mathbf{A}`` matrices for the inequality constraints: ```math \begin{bmatrix} \mathbf{A_{W_{min}}} \\ \mathbf{A_{W_{max}}} \end{bmatrix} \mathbf{Z̃} ≤ \begin{bmatrix} - - \mathbf{W_{min} + F_W} \\ - + \mathbf{W_{max} - F_W} + - \mathbf{W_{min} + F_w} \\ + + \mathbf{W_{max} - F_w} \end{bmatrix} ``` """ diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 4e14bc16a..a27644a13 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -773,7 +773,7 @@ Set `b` vector for the linear model inequality constraints (``\mathbf{A Z̃ ≤ Also init ``\mathbf{f_x̂} = \mathbf{g_x̂ d_0}(k) + \mathbf{j_x̂ D̂_0} + \mathbf{k_x̂ x̂_0}(k) + \mathbf{v_x̂ u_0}(k-1) + \mathbf{b_x̂}`` vector for the terminal constraints, see -[`init_predmat`](@ref). The ``\mathbf{F_W}`` vector for the custom linear constraints is +[`init_predmat`](@ref). The ``\mathbf{F_w}`` vector for the custom linear constraints is also updated, see [`relaxW`](@ref). """ function linconstraint!(mpc::PredictiveController, model::LinModel, ::TranscriptionMethod) @@ -868,7 +868,7 @@ function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::SingleShooti return nothing end -"Init the ``\\mathbf{F_W}`` vector for the linear model custom inequality constraints." +"Init the ``\\mathbf{F_w}`` vector for the linear model custom inequality constraints." function linconstraint_custom!(mpc::PredictiveController, model::SimModel) ny, nu, nd, buffer = model.ny, model.nu, model.nd, mpc.buffer Fw = mpc.con.Fw @@ -879,7 +879,7 @@ function linconstraint_custom!(mpc::PredictiveController, model::SimModel) mul!(Fw, mpc.con.W̄u, Ue_term, 1, 1) if model.nd > 0 D̂e_term[1:nd] .= mpc.d0 .+ model.dop - D̂e_term[nd+1:end] .= mpc.D̂0 .+ model.D̂op + D̂e_term[nd+1:end] .= mpc.D̂0 .+ mpc.Dop mul!(Fw, mpc.con.W̄d, D̂e_term, 1, 1) end R̂e_term[1:ny] .= mpc.ry From afcbe7d89a1466c493af1682ee2b5d7261b97468 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 15:47:17 -0500 Subject: [PATCH 25/43] test: debug new test with custom linear constraints --- test/3_test_predictive_control.jl | 32 +++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 7de68fdef..8039addd5 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -6,9 +6,9 @@ @test size(mpc1.Ẽ,1) == 15*mpc1.estim.model.ny mpc2 = LinMPC(model, Hc=4, Cwt=Inf) @test size(mpc2.Ẽ,2) == 4*mpc2.estim.model.nu - mpc3 = LinMPC(model, Hc=4, Cwt=1e6) + mpc3 = LinMPC(model, Hc=4, Cwt=1e4) @test size(mpc3.Ẽ,2) == 4*mpc3.estim.model.nu + 1 - @test mpc3.weights.Ñ_Hc[end] ≈ 1e6 + @test mpc3.weights.Ñ_Hc[end] ≈ 1e4 mpc4 = LinMPC(model, Mwt=[1,2], Hp=15) @test mpc4.weights.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15))) @test mpc4.weights.M_Hp isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}} @@ -53,14 +53,12 @@ mpc16 = LinMPC(model, Hc=[1,2,3,6,6,6], Hp=10, Cwt=Inf) @test mpc16.Hc == 4 # the last 2 elements of Hc are ignored @test size(mpc16.P̃u) == (10*mpc1.estim.model.nu, 4*mpc1.estim.model.nu) - - Wy = ones(3, ny) + Wy = ones(3, model.ny) mpc17 = LinMPC(model; Wy) @test mpc17.con.W̄y ≈ ModelPredictiveControl.repeatdiag(Wy, mpc17.Hp+1) - @test mpc17.con.W̄u ≈ zeros(model.nu*(mpc17.Hp+1), model.nu*(mpc17.Hp+1)) - @test mpc17.con.W̄d ≈ zeros(model.nd*(mpc17.Hp+1), model.nd*(mpc17.Hp+1)) - @test mpc17.con.W̄r ≈ zeros(model.ny*(mpc17.Hp+1), model.ny*(mpc17.Hp+1)) - + @test mpc17.con.W̄u ≈ zeros(3*(mpc17.Hp+1), model.nu*(mpc17.Hp+1)) + @test mpc17.con.W̄d ≈ zeros(3*(mpc17.Hp+1), model.nd*(mpc17.Hp+1)) + @test mpc17.con.W̄r ≈ zeros(3*(mpc17.Hp+1), model.ny*(mpc17.Hp+1)) Wy = ones(2, model.ny) Wu = 2*ones(2, model.nu) Wd = 3*ones(2, model.nd) @@ -87,11 +85,11 @@ @test_throws ArgumentError LinMPC(model, Nwt=[-1,1]) @test_throws ArgumentError LinMPC(model, Lwt=[-1,1]) @test_throws ArgumentError LinMPC(model, Cwt=-1) - @test_throws DimensionMismatch LinMPC(model, Wy=ones(2, ny+1)) - @test_throws DimensionMismatch LinMPC(model, Wu=ones(2, nu-1)) - @test_throws DimensionMismatch LinMPC(model, Wd=ones(2, nd+1)) - @test_throws DimensionMismatch LinMPC(model, Wr=ones(2, ny-1)) - @test_throws DimensionMismatch LinMPC(model, Wy=ones(2, ny), Wu=ones(3, nu)) + @test_throws DimensionMismatch LinMPC(model, Wy=ones(2, model.ny+1)) + @test_throws DimensionMismatch LinMPC(model, Wu=ones(2, model.nu-1)) + @test_throws DimensionMismatch LinMPC(model, Wd=ones(2, model.nd+1)) + @test_throws DimensionMismatch LinMPC(model, Wr=ones(2, model.ny-1)) + @test_throws DimensionMismatch LinMPC(model, Wy=ones(2, model.ny), Wu=ones(3, model.nu)) end @testitem "LinMPC moves and getinfo" setup=[SetupMPCtests] begin @@ -264,16 +262,18 @@ end @testitem "LinMPC set constraints" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra model = LinModel(sys, Ts, i_d=[3]) - mpc = LinMPC(model, Hp=1, Hc=1) + mpc = LinMPC(model, Hp=1, Hc=1, Wr=ones(2, 2)) # test default constraints before modifying any: @test all((mpc.con.U0min, mpc.con.U0max) .≈ (fill(-Inf, model.nu), fill(Inf, model.nu))) @test all((mpc.con.ΔŨmin, mpc.con.ΔŨmax) .≈ (vcat(fill(-Inf, model.nu), 0), vcat(fill(Inf, model.nu), Inf))) @test all((mpc.con.Y0min, mpc.con.Y0max) .≈ (fill(-Inf, model.ny), fill(Inf, model.ny))) + @test all((mpc.con.Wmin, mpc.con.Wmax) .≈ (fill(-Inf, 2mpc.con.nw), fill(Inf, 2mpc.con.nw))) @test all((mpc.con.x̂0min, mpc.con.x̂0max) .≈ (fill(-Inf, mpc.estim.nx̂), fill(Inf, mpc.estim.nx̂))) @test all((-mpc.con.A_Umin[:, end], -mpc.con.A_Umax[:, end]) .≈ (fill(0.0, model.nu), fill(0.0, model.nu))) @test all((-mpc.con.A_ΔŨmin[1:end-1, end], -mpc.con.A_ΔŨmax[1:end-1, end]) .≈ (fill(0.0, model.nu), fill(0.0, model.nu))) @test all((-mpc.con.A_Ymin[:, end], -mpc.con.A_Ymax[:, end]) .≈ (fill(1.0, model.ny), fill(1.0, model.ny))) + @test all((-mpc.con.A_Wmin[:, end], -mpc.con.A_Wmax[:, end]) .≈ (fill(1.0, 2mpc.con.nw), fill(1.0, 2mpc.con.nw))) @test all((-mpc.con.A_x̂min[:, end], -mpc.con.A_x̂max[:, end]) .≈ (fill(1.0, mpc.estim.nx̂), fill(1.0, mpc.estim.nx̂))) setconstraint!(mpc, umin=[-5, -9.9], umax=[100,99]) @@ -697,9 +697,9 @@ end @test size(nmpc1.R̂y, 1) == 15*nmpc1.estim.model.ny nmpc2 = NonLinMPC(nonlinmodel, Hp=15, Hc=4, Cwt=Inf) @test size(nmpc2.Ẽ, 2) == 4*nonlinmodel.nu - nmpc3 = NonLinMPC(nonlinmodel, Hp=15, Hc=4, Cwt=1e6) + nmpc3 = NonLinMPC(nonlinmodel, Hp=15, Hc=4, Cwt=1e4) @test size(nmpc3.Ẽ, 2) == 4*nonlinmodel.nu + 1 - @test nmpc3.weights.Ñ_Hc[end] == 1e6 + @test nmpc3.weights.Ñ_Hc[end] == 1e4 nmpc4 = NonLinMPC(nonlinmodel, Hp=15, Mwt=[1,2]) @test nmpc4.weights.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15))) @test nmpc4.weights.M_Hp isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}} From 2d8879130c2f4b4ef7e6e5e9e26089fc8a9f7e83 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 15:49:07 -0500 Subject: [PATCH 26/43] test: avoiding `all` in `approx` tests The error message is clearer if the tests do not pass. --- test/3_test_predictive_control.jl | 42 ++++++++++++++++++------------- 1 file changed, 25 insertions(+), 17 deletions(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 8039addd5..05999c0aa 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -1035,19 +1035,21 @@ end nmpc_lin = NonLinMPC(linmodel1, Hp=1, Hc=1) setconstraint!(nmpc_lin, ymin=[5,10],ymax=[55, 35]) - @test all((nmpc_lin.con.Y0min, nmpc_lin.con.Y0max) .≈ ([5,10], [55,35])) + @test nmpc_lin.con.Y0min ≈ [5,10] + @test nmpc_lin.con.Y0max ≈ [55, 35] setconstraint!(nmpc_lin, c_ymin=[1.0,1.1], c_ymax=[1.2,1.3]) - @test all((-nmpc_lin.con.A_Ymin[:, end], -nmpc_lin.con.A_Ymax[:, end]) .≈ - ([1.0,1.1], [1.2,1.3])) + @test -nmpc_lin.con.A_Ymin[:, end] ≈ [1.0,1.1] + @test -nmpc_lin.con.A_Ymax[:, end] ≈ [1.2,1.3] setconstraint!(nmpc_lin, x̂min=[-21,-22,-23,-24,-25,-26], x̂max=[21,22,23,24,25,26]) - @test all((nmpc_lin.con.x̂0min, nmpc_lin.con.x̂0max) .≈ - ([-21,-22,-23,-24,-25,-26], [21,22,23,24,25,26])) + @test nmpc_lin.con.x̂0min ≈ [-21,-22,-23,-24,-25,-26] + @test nmpc_lin.con.x̂0max ≈ [21,22,23,24,25,26] setconstraint!(nmpc_lin, c_x̂min=[0.21,0.22,0.23,0.24,0.25,0.26], c_x̂max=[0.31,0.32,0.33,0.34,0.35,0.36] ) - @test all((-nmpc_lin.con.A_x̂min[:, end], -nmpc_lin.con.A_x̂max[:, end]) .≈ - ([0.21,0.22,0.23,0.24,0.25,0.26], [0.31,0.32,0.33,0.34,0.35,0.36])) + @test -nmpc_lin.con.A_x̂min[:, end] ≈ [0.21,0.22,0.23,0.24,0.25,0.26] + @test -nmpc_lin.con.A_x̂max[:, end] ≈ [0.31,0.32,0.33,0.34,0.35,0.36] + @test -nmpc_lin.con.A_x̂max[:, end] ≈ [0.31,0.32,0.33,0.34,0.35,0.36] f = (x,u,d,_) -> linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d h = (x,d,_) -> linmodel1.C*x + linmodel1.Dd*d @@ -1124,23 +1126,29 @@ end nmpc_ms = NonLinMPC(nonlinmodel, Hp=1, Hc=1, transcription=MultipleShooting()) - setconstraint!(nmpc_ms, ymin=[-6, -11],ymax=[55, 35]) - @test all((nmpc_ms.con.Y0min, nmpc_ms.con.Y0max) .≈ ([-6,-11], [55,35])) + setconnmpc_ms.con.Y0min ≈ [-6,-11] + @test nmpc_ms.con.Y0max ≈ [55,35] setconstraint!(nmpc_ms, x̂min=[-21,-22,-23,-24,-25,-26], x̂max=[21,22,23,24,25,26]) - @test all((nmpc_ms.con.x̂0min, nmpc_ms.con.x̂0max) .≈ - ([-21,-22,-23,-24,-25,-26], [21,22,23,24,25,26])) + @test nmpc_ms.con.x̂0min ≈ [-21,-22,-23,-24,-25,-26] + @test nmpc_ms.con.x̂0max ≈ [21,22,23,24,25,26] setconstraint!(nmpc_ms, c_ymin=[1.00,1.01], c_ymax=[1.02,1.03]) - @test all((-nmpc_ms.con.A_Ymin, -nmpc_ms.con.A_Ymax) .≈ (zeros(0,9), zeros(0,9))) - @test all((nmpc_ms.con.C_ymin, nmpc_ms.con.C_ymax) .≈ ([1.00,1.01], [1.02,1.03])) + @test -nmpc_ms.con.A_Ymin ≈ zeros(0,9) + @test -nmpc_ms.con.A_Ymax ≈ zeros(0,9) + @test nmpc_ms.con.C_ymin ≈ [1.00,1.01] + @test nmpc_ms.con.C_ymax ≈ [1.02,1.03] setconstraint!(nmpc_ms, c_x̂min=[0.21,0.22,0.23,0.24,0.25,0.26], c_x̂max=[0.31,0.32,0.33,0.34,0.35,0.36] ) - @test all((-nmpc_lin.con.A_x̂min[:, end], -nmpc_lin.con.A_x̂max[:, end]) .≈ - ([0.21,0.22,0.23,0.24,0.25,0.26], [0.31,0.32,0.33,0.34,0.35,0.36])) - @test all((nmpc_ms.con.c_x̂min, nmpc_ms.con.c_x̂max) .≈ - ([0.21,0.22,0.23,0.24,0.25,0.26], [0.31,0.32,0.33,0.34,0.35,0.36])) + @test -nmpc_lin.con.A_x̂min[:, end] ≈ [0.21,0.22,0.23,0.24,0.25,0.26] + @test -nmpc_lin.con.A_x̂max[:, end] ≈ [0.31,0.32,0.33,0.34,0.35,0.36] + @test nmpc_ms.con.c_x̂min ≈ [0.21,0.22,0.23,0.24,0.25,0.26] + @test nmpc_ms.con.c_x̂max ≈ [0.31,0.32,0.33,0.34,0.35,0.36] + @test -nmpc_lin.con.A_x̂min[:, end] ≈ [0.21,0.22,0.23,0.24,0.25,0.26] + @test -nmpc_lin.con.A_x̂max[:, end] ≈ [0.31,0.32,0.33,0.34,0.35,0.36] + @test nmpc_ms.con.c_x̂min ≈ [0.21,0.22,0.23,0.24,0.25,0.26] + @test nmpc_ms.con.c_x̂max ≈ [0.31,0.32,0.33,0.34,0.35,0.36] end From 9014c47c07fbedd2e7e4814f4c53bb7877aa1f52 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 16:11:51 -0500 Subject: [PATCH 27/43] test: idem (with copilot) --- test/2_test_state_estim.jl | 60 ++++++++----- test/3_test_predictive_control.jl | 144 ++++++++++++++++++------------ 2 files changed, 127 insertions(+), 77 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 55a904d7d..3fff7327b 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1163,35 +1163,51 @@ end linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) mhe1 = MovingHorizonEstimator(linmodel, He=1, nint_ym=0, Cwt=1e3) setconstraint!(mhe1, x̂min=[-51,-52], x̂max=[53,54]) - @test all((mhe1.con.X̂0min, mhe1.con.X̂0max) .≈ ([-51,-52], [53,54])) - @test all((mhe1.con.x̃0min[2:end], mhe1.con.x̃0max[2:end]) .≈ ([-51,-52], [53,54])) + @test mhe1.con.X̂0min ≈ [-51,-52] + @test mhe1.con.X̂0max ≈ [53,54] + @test mhe1.con.x̃0min[2:end] ≈ [-51,-52] + @test mhe1.con.x̃0max[2:end] ≈ [53,54] setconstraint!(mhe1, ŵmin=[-55,-56], ŵmax=[57,58]) - @test all((mhe1.con.Ŵmin, mhe1.con.Ŵmax) .≈ ([-55,-56], [57,58])) + @test mhe1.con.Ŵmin ≈ [-55,-56] + @test mhe1.con.Ŵmax ≈ [57,58] setconstraint!(mhe1, v̂min=[-59,-60], v̂max=[61,62]) - @test all((mhe1.con.V̂min, mhe1.con.V̂max) .≈ ([-59,-60], [61,62])) + @test mhe1.con.V̂min ≈ [-59,-60] + @test mhe1.con.V̂max ≈ [61,62] setconstraint!(mhe1, c_x̂min=[0.01,0.02], c_x̂max=[0.03,0.04]) - @test all((-mhe1.con.A_X̂min[:, end], -mhe1.con.A_X̂max[:, end]) .≈ ([0.01, 0.02], [0.03,0.04])) - @test all((-mhe1.con.A_x̃min[2:end, end], -mhe1.con.A_x̃max[2:end, end]) .≈ ([0.01,0.02], [0.03,0.04])) + @test -mhe1.con.A_X̂min[:, end] ≈ [0.01, 0.02] + @test -mhe1.con.A_X̂max[:, end] ≈ [0.03,0.04] + @test -mhe1.con.A_x̃min[2:end, end] ≈ [0.01,0.02] + @test -mhe1.con.A_x̃max[2:end, end] ≈ [0.03,0.04] setconstraint!(mhe1, c_ŵmin=[0.05,0.06], c_ŵmax=[0.07,0.08]) - @test all((-mhe1.con.A_Ŵmin[:, end], -mhe1.con.A_Ŵmax[:, end]) .≈ ([0.05, 0.06], [0.07,0.08])) + @test -mhe1.con.A_Ŵmin[:, end] ≈ [0.05, 0.06] + @test -mhe1.con.A_Ŵmax[:, end] ≈ [0.07,0.08] setconstraint!(mhe1, c_v̂min=[0.09,0.10], c_v̂max=[0.11,0.12]) - @test all((-mhe1.con.A_V̂min[:, end], -mhe1.con.A_V̂max[:, end]) .≈ ([0.09, 0.10], [0.11,0.12])) + @test -mhe1.con.A_V̂min[:, end] ≈ [0.09, 0.10] + @test -mhe1.con.A_V̂max[:, end] ≈ [0.11,0.12] mhe2 = MovingHorizonEstimator(linmodel, He=4, nint_ym=0, Cwt=1e3) setconstraint!(mhe2, X̂min=-1(1:10), X̂max=1(1:10)) - @test all((mhe2.con.X̂0min, mhe2.con.X̂0max) .≈ (-1(3:10), 1(3:10))) - @test all((mhe2.con.x̃0min[2:end], mhe2.con.x̃0max[2:end]) .≈ (-1(1:2), 1(1:2))) + @test mhe2.con.X̂0min ≈ -1(3:10) + @test mhe2.con.X̂0max ≈ 1(3:10) + @test mhe2.con.x̃0min[2:end] ≈ -1(1:2) + @test mhe2.con.x̃0max[2:end] ≈ 1(1:2) setconstraint!(mhe2, Ŵmin=-1(11:18), Ŵmax=1(11:18)) - @test all((mhe2.con.Ŵmin, mhe2.con.Ŵmax) .≈ (-1(11:18), 1(11:18))) + @test mhe2.con.Ŵmin ≈ -1(11:18) + @test mhe2.con.Ŵmax ≈ 1(11:18) setconstraint!(mhe2, V̂min=-1(31:38), V̂max=1(31:38)) - @test all((mhe2.con.V̂min, mhe2.con.V̂max) .≈ (-1(31:38), 1(31:38))) + @test mhe2.con.V̂min ≈ -1(31:38) + @test mhe2.con.V̂max ≈ 1(31:38) setconstraint!(mhe2, C_x̂min=0.01(1:10), C_x̂max=0.02(1:10)) - @test all((-mhe2.con.A_X̂min[:, end], -mhe2.con.A_X̂max[:, end]) .≈ (0.01(3:10), 0.02(3:10))) - @test all((-mhe2.con.A_x̃min[2:end, end], -mhe2.con.A_x̃max[2:end, end]) .≈ (0.01(1:2), 0.02(1:2))) + @test -mhe2.con.A_X̂min[:, end] ≈ 0.01(3:10) + @test -mhe2.con.A_X̂max[:, end] ≈ 0.02(3:10) + @test -mhe2.con.A_x̃min[2:end, end] ≈ 0.01(1:2) + @test -mhe2.con.A_x̃max[2:end, end] ≈ 0.02(1:2) setconstraint!(mhe2, C_ŵmin=0.03(11:18), C_ŵmax=0.04(11:18)) - @test all((-mhe2.con.A_Ŵmin[:, end], -mhe2.con.A_Ŵmax[:, end]) .≈ (0.03(11:18), 0.04(11:18))) + @test -mhe2.con.A_Ŵmin[:, end] ≈ 0.03(11:18) + @test -mhe2.con.A_Ŵmax[:, end] ≈ 0.04(11:18) setconstraint!(mhe2, C_v̂min=0.05(31:38), C_v̂max=0.06(31:38)) - @test all((-mhe2.con.A_V̂min[:, end], -mhe2.con.A_V̂max[:, end]) .≈ (0.05(31:38), 0.06(31:38))) + @test -mhe2.con.A_V̂min[:, end] ≈ 0.05(31:38) + @test -mhe2.con.A_V̂max[:, end] ≈ 0.06(31:38) f(x,u,d,model) = model.A*x + model.Bu*u h(x,d,model) = model.C*x @@ -1200,16 +1216,20 @@ end mhe3 = MovingHorizonEstimator(nonlinmodel, He=4, nint_ym=0, Cwt=1e3) setconstraint!(mhe3, C_x̂min=0.01(1:10), C_x̂max=0.02(1:10)) - @test all((mhe3.con.C_x̂min, mhe3.con.C_x̂max) .≈ (0.01(3:10), 0.02(3:10))) + @test mhe3.con.C_x̂min ≈ 0.01(3:10) + @test mhe3.con.C_x̂max ≈ 0.02(3:10) setconstraint!(mhe3, C_v̂min=0.03(11:18), C_v̂max=0.04(11:18)) - @test all((mhe3.con.C_v̂min, mhe3.con.C_v̂max) .≈ (0.03(11:18), 0.04(11:18))) + @test mhe3.con.C_v̂min ≈ 0.03(11:18) + @test mhe3.con.C_v̂max ≈ 0.04(11:18) # TODO: delete these tests when the deprecated legacy splatting syntax will be. mhe4 = MovingHorizonEstimator(nonlinmodel, He=4, nint_ym=0, Cwt=1e3, oracle=false) setconstraint!(mhe3, C_x̂min=0.01(1:10), C_x̂max=0.02(1:10)) - @test all((mhe3.con.C_x̂min, mhe3.con.C_x̂max) .≈ (0.01(3:10), 0.02(3:10))) + @test mhe3.con.C_x̂min ≈ 0.01(3:10) + @test mhe3.con.C_x̂max ≈ 0.02(3:10) setconstraint!(mhe3, C_v̂min=0.03(11:18), C_v̂max=0.04(11:18)) - @test all((mhe3.con.C_v̂min, mhe3.con.C_v̂max) .≈ (0.03(11:18), 0.04(11:18))) + @test mhe3.con.C_v̂min ≈ 0.03(11:18) + @test mhe3.con.C_v̂max ≈ 0.04(11:18) @test_throws ArgumentError setconstraint!(mhe2, x̂min=[-1]) @test_throws ArgumentError setconstraint!(mhe2, x̂max=[+1]) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 05999c0aa..4f1d8c16d 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -265,51 +265,75 @@ end mpc = LinMPC(model, Hp=1, Hc=1, Wr=ones(2, 2)) # test default constraints before modifying any: - @test all((mpc.con.U0min, mpc.con.U0max) .≈ (fill(-Inf, model.nu), fill(Inf, model.nu))) - @test all((mpc.con.ΔŨmin, mpc.con.ΔŨmax) .≈ (vcat(fill(-Inf, model.nu), 0), vcat(fill(Inf, model.nu), Inf))) - @test all((mpc.con.Y0min, mpc.con.Y0max) .≈ (fill(-Inf, model.ny), fill(Inf, model.ny))) - @test all((mpc.con.Wmin, mpc.con.Wmax) .≈ (fill(-Inf, 2mpc.con.nw), fill(Inf, 2mpc.con.nw))) - @test all((mpc.con.x̂0min, mpc.con.x̂0max) .≈ (fill(-Inf, mpc.estim.nx̂), fill(Inf, mpc.estim.nx̂))) - @test all((-mpc.con.A_Umin[:, end], -mpc.con.A_Umax[:, end]) .≈ (fill(0.0, model.nu), fill(0.0, model.nu))) - @test all((-mpc.con.A_ΔŨmin[1:end-1, end], -mpc.con.A_ΔŨmax[1:end-1, end]) .≈ (fill(0.0, model.nu), fill(0.0, model.nu))) - @test all((-mpc.con.A_Ymin[:, end], -mpc.con.A_Ymax[:, end]) .≈ (fill(1.0, model.ny), fill(1.0, model.ny))) - @test all((-mpc.con.A_Wmin[:, end], -mpc.con.A_Wmax[:, end]) .≈ (fill(1.0, 2mpc.con.nw), fill(1.0, 2mpc.con.nw))) - @test all((-mpc.con.A_x̂min[:, end], -mpc.con.A_x̂max[:, end]) .≈ (fill(1.0, mpc.estim.nx̂), fill(1.0, mpc.estim.nx̂))) + @test mpc.con.U0min ≈ fill(-Inf, model.nu) + @test mpc.con.U0max ≈ fill(Inf, model.nu) + @test mpc.con.ΔŨmin ≈ vcat(fill(-Inf, model.nu), 0) + @test mpc.con.ΔŨmax ≈ vcat(fill(Inf, model.nu), Inf) + @test mpc.con.Y0min ≈ fill(-Inf, model.ny) + @test mpc.con.Y0max ≈ fill(Inf, model.ny) + @test mpc.con.Wmin ≈ fill(-Inf, 2mpc.con.nw) + @test mpc.con.Wmax ≈ fill(Inf, 2mpc.con.nw) + @test mpc.con.x̂0min ≈ fill(-Inf, mpc.estim.nx̂) + @test mpc.con.x̂0max ≈ fill(Inf, mpc.estim.nx̂) + @test -mpc.con.A_Umin[:, end] ≈ fill(0.0, model.nu) + @test -mpc.con.A_Umax[:, end] ≈ fill(0.0, model.nu) + @test -mpc.con.A_ΔŨmin[1:end-1, end] ≈ fill(0.0, model.nu) + @test -mpc.con.A_ΔŨmax[1:end-1, end] ≈ fill(0.0, model.nu) + @test -mpc.con.A_Ymin[:, end] ≈ fill(1.0, model.ny) + @test -mpc.con.A_Ymax[:, end] ≈ fill(1.0, model.ny) + @test -mpc.con.A_Wmin[:, end] ≈ fill(1.0, 2mpc.con.nw) + @test -mpc.con.A_Wmax[:, end] ≈ fill(1.0, 2mpc.con.nw) + @test -mpc.con.A_x̂min[:, end] ≈ fill(1.0, mpc.estim.nx̂) + @test -mpc.con.A_x̂max[:, end] ≈ fill(1.0, mpc.estim.nx̂) setconstraint!(mpc, umin=[-5, -9.9], umax=[100,99]) - @test all((mpc.con.U0min, mpc.con.U0max) .≈ ([-5, -9.9], [100,99])) + @test mpc.con.U0min ≈ [-5, -9.9] + @test mpc.con.U0max ≈ [100,99] setconstraint!(mpc, Δumin=[-5,-10], Δumax=[6,11]) - @test all((mpc.con.ΔŨmin, mpc.con.ΔŨmax) .≈ ([-5,-10,0], [6,11,Inf])) + @test mpc.con.ΔŨmin ≈ [-5,-10,0] + @test mpc.con.ΔŨmax ≈ [6,11,Inf] setconstraint!(mpc, ymin=[-6, -11],ymax=[55, 35]) - @test all((mpc.con.Y0min, mpc.con.Y0max) .≈ ([-6,-11], [55,35])) + @test mpc.con.Y0min ≈ [-6,-11] + @test mpc.con.Y0max ≈ [55,35] setconstraint!(mpc, x̂min=[-21,-22,-23,-24,-25,-26], x̂max=[21,22,23,24,25,26]) - @test all((mpc.con.x̂0min, mpc.con.x̂0max) .≈ ([-21,-22,-23,-24,-25,-26], [21,22,23,24,25,26])) + @test mpc.con.x̂0min ≈ [-21,-22,-23,-24,-25,-26] + @test mpc.con.x̂0max ≈ [21,22,23,24,25,26] setconstraint!(mpc, c_umin=[0.01,0.02], c_umax=[0.03,0.04]) - @test all((-mpc.con.A_Umin[:, end], -mpc.con.A_Umax[:, end]) .≈ ([0.01,0.02], [0.03,0.04])) + @test -mpc.con.A_Umin[:, end] ≈ [0.01,0.02] + @test -mpc.con.A_Umax[:, end] ≈ [0.03,0.04] setconstraint!(mpc, c_Δumin=[0.05,0.06], c_Δumax=[0.07,0.08]) - @test all((-mpc.con.A_ΔŨmin[1:end-1, end], -mpc.con.A_ΔŨmax[1:end-1, end]) .≈ ([0.05,0.06], [0.07,0.08])) + @test -mpc.con.A_ΔŨmin[1:end-1, end] ≈ [0.05,0.06] + @test -mpc.con.A_ΔŨmax[1:end-1, end] ≈ [0.07,0.08] setconstraint!(mpc, c_ymin=[1.00,1.01], c_ymax=[1.02,1.03]) - @test all((-mpc.con.A_Ymin[:, end], -mpc.con.A_Ymax[:, end]) .≈ ([1.00,1.01], [1.02,1.03])) + @test -mpc.con.A_Ymin[:, end] ≈ [1.00,1.01] + @test -mpc.con.A_Ymax[:, end] ≈ [1.02,1.03] setconstraint!(mpc, c_x̂min=[0.21,0.22,0.23,0.24,0.25,0.26], c_x̂max=[0.31,0.32,0.33,0.34,0.35,0.36]) - @test all((-mpc.con.A_x̂min[:, end], -mpc.con.A_x̂max[:, end]) .≈ ([0.21,0.22,0.23,0.24,0.25,0.26], [0.31,0.32,0.33,0.34,0.35,0.36])) + @test -mpc.con.A_x̂min[:, end] ≈ [0.21,0.22,0.23,0.24,0.25,0.26] + @test -mpc.con.A_x̂max[:, end] ≈ [0.31,0.32,0.33,0.34,0.35,0.36] model2 = LinModel(tf([2], [10, 1]), 3.0) mpc2 = LinMPC(model2, Hp=50, Hc=5) setconstraint!(mpc2, Umin=-1(1:50).-1, Umax=+1(1:50).+1) - @test all((mpc2.con.U0min, mpc2.con.U0max) .≈ (-1(1:50).-1, +1(1:50).+1)) + @test mpc2.con.U0min ≈ -1(1:50).-1 + @test mpc2.con.U0max ≈ +1(1:50).+1 setconstraint!(mpc2, ΔUmin=-1(1:5).-2, ΔUmax=+1(1:5).+2) - @test all((mpc2.con.ΔŨmin, mpc2.con.ΔŨmax) .≈ ([-1(1:5).-2; 0], [+1(1:5).+2; Inf])) + @test mpc2.con.ΔŨmin ≈ [-1(1:5).-2; 0] + @test mpc2.con.ΔŨmax ≈ [+1(1:5).+2; Inf] setconstraint!(mpc2, Ymin=-1(1:50).-3, Ymax=+1(1:50).+3) - @test all((mpc2.con.Y0min, mpc2.con.Y0max) .≈ (-1(1:50).-3, +1(1:50).+3)) + @test mpc2.con.Y0min ≈ -1(1:50).-3 + @test mpc2.con.Y0max ≈ +1(1:50).+3 setconstraint!(mpc2, C_umin=+1(1:50).+4, C_umax=+1(1:50).+4) - @test all((-mpc2.con.A_Umin[:, end], -mpc2.con.A_Umax[:, end]) .≈ (+1(1:50).+4, +1(1:50).+4)) + @test -mpc2.con.A_Umin[:, end] ≈ +1(1:50).+4 + @test -mpc2.con.A_Umax[:, end] ≈ +1(1:50).+4 setconstraint!(mpc2, C_Δumin=+1(1:5).+5, C_Δumax=+1(1:5).+5) - @test all((-mpc2.con.A_ΔŨmin[1:end-1, end], -mpc2.con.A_ΔŨmax[1:end-1, end]) .≈ (+1(1:5).+5, +1(1:5).+5)) + @test -mpc2.con.A_ΔŨmin[1:end-1, end] ≈ +1(1:5).+5 + @test -mpc2.con.A_ΔŨmax[1:end-1, end] ≈ +1(1:5).+5 setconstraint!(mpc2, C_ymin=+1(1:50).+6, C_ymax=+1(1:50).+6) - @test all((-mpc2.con.A_Ymin[:, end], -mpc2.con.A_Ymax[:, end]) .≈ (+1(1:50).+6, +1(1:50).+6)) + @test -mpc2.con.A_Ymin[:, end] ≈ +1(1:50).+6 + @test -mpc2.con.A_Ymax[:, end] ≈ +1(1:50).+6 setconstraint!(mpc2, c_umin=[0], c_umax=[0], c_Δumin=[0], c_Δumax=[0], c_ymin=[1], c_ymax=[1]) @test_throws ArgumentError setconstraint!(mpc, umin=[0,0,0]) @@ -1036,7 +1060,7 @@ end setconstraint!(nmpc_lin, ymin=[5,10],ymax=[55, 35]) @test nmpc_lin.con.Y0min ≈ [5,10] - @test nmpc_lin.con.Y0max ≈ [55, 35] + @test nmpc_lin.con.Y0max ≈ [55,35] setconstraint!(nmpc_lin, c_ymin=[1.0,1.1], c_ymax=[1.2,1.3]) @test -nmpc_lin.con.A_Ymin[:, end] ≈ [1.0,1.1] @test -nmpc_lin.con.A_Ymax[:, end] ≈ [1.2,1.3] @@ -1049,7 +1073,6 @@ end ) @test -nmpc_lin.con.A_x̂min[:, end] ≈ [0.21,0.22,0.23,0.24,0.25,0.26] @test -nmpc_lin.con.A_x̂max[:, end] ≈ [0.31,0.32,0.33,0.34,0.35,0.36] - @test -nmpc_lin.con.A_x̂max[:, end] ≈ [0.31,0.32,0.33,0.34,0.35,0.36] f = (x,u,d,_) -> linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d h = (x,d,_) -> linmodel1.C*x + linmodel1.Dd*d @@ -1057,30 +1080,35 @@ end nmpc = NonLinMPC(nonlinmodel, Hp=1, Hc=1) setconstraint!(nmpc, umin=[-5, -9.9], umax=[100,99]) - @test all((nmpc.con.U0min, nmpc.con.U0max) .≈ ([-5, -9.9], [100,99])) + @test nmpc.con.U0min ≈ [-5, -9.9] + @test nmpc.con.U0max ≈ [100,99] setconstraint!(nmpc, Δumin=[-5,-10], Δumax=[6,11]) - @test all((nmpc.con.ΔŨmin, nmpc.con.ΔŨmax) .≈ ([-5,-10,0], [6,11,Inf])) + @test nmpc.con.ΔŨmin ≈ [-5,-10,0] + @test nmpc.con.ΔŨmax ≈ [6,11,Inf] setconstraint!(nmpc, ymin=[-6, -11],ymax=[55, 35]) - @test all((nmpc.con.Y0min, nmpc.con.Y0max) .≈ ([-6,-11], [55,35])) + @test nmpc.con.Y0min ≈ [-6,-11] + @test nmpc.con.Y0max ≈ [55,35] setconstraint!(nmpc, x̂min=[-21,-22,-23,-24,-25,-26], x̂max=[21,22,23,24,25,26]) - @test all((nmpc.con.x̂0min, nmpc.con.x̂0max) .≈ - ([-21,-22,-23,-24,-25,-26], [21,22,23,24,25,26])) + @test nmpc.con.x̂0min ≈ [-21,-22,-23,-24,-25,-26] + @test nmpc.con.x̂0max ≈ [21,22,23,24,25,26] setconstraint!(nmpc, c_umin=[0.01,0.02], c_umax=[0.03,0.04]) - @test all((-nmpc.con.A_Umin[:, end], -nmpc.con.A_Umax[:, end]) .≈ - ([0.01,0.02], [0.03,0.04])) + @test -nmpc.con.A_Umin[:, end] ≈ [0.01,0.02] + @test -nmpc.con.A_Umax[:, end] ≈ [0.03,0.04] setconstraint!(nmpc, c_Δumin=[0.05,0.06], c_Δumax=[0.07,0.08]) - @test all((-nmpc.con.A_ΔŨmin[1:end-1, end], -nmpc.con.A_ΔŨmax[1:end-1, end]) .≈ - ([0.05,0.06], [0.07,0.08])) + @test -nmpc.con.A_ΔŨmin[1:end-1, end] ≈ [0.05,0.06] + @test -nmpc.con.A_ΔŨmax[1:end-1, end] ≈ [0.07,0.08] setconstraint!(nmpc, c_ymin=[1.00,1.01], c_ymax=[1.02,1.03]) - @test all((-nmpc.con.A_Ymin, -nmpc.con.A_Ymax) .≈ (zeros(0,3), zeros(0,3))) - @test all((nmpc.con.C_ymin, nmpc.con.C_ymax) .≈ ([1.00,1.01], [1.02,1.03])) + @test -nmpc.con.A_Ymin ≈ zeros(0,3) + @test -nmpc.con.A_Ymax ≈ zeros(0,3) + @test nmpc.con.C_ymin ≈ [1.00,1.01] + @test nmpc.con.C_ymax ≈ [1.02,1.03] setconstraint!(nmpc, c_x̂min=[0.21,0.22,0.23,0.24,0.25,0.26], c_x̂max=[0.31,0.32,0.33,0.34,0.35,0.36] ) - @test all((nmpc.con.c_x̂min, nmpc.con.c_x̂max) .≈ - ([0.21,0.22,0.23,0.24,0.25,0.26], [0.31,0.32,0.33,0.34,0.35,0.36])) + @test nmpc.con.c_x̂min ≈ [0.21,0.22,0.23,0.24,0.25,0.26] + @test nmpc.con.c_x̂max ≈ [0.31,0.32,0.33,0.34,0.35,0.36] # TODO: delete these tests when the deprecated legacy splatting syntax will be. gc_leg! = (LHS,_,_,_,_,_) -> (LHS[begin] = -1) @@ -1099,34 +1127,40 @@ end nmpc_leg = NonLinMPC(nonlinmodel, Hp=1, Hc=1, oracle=false) setconstraint!(nmpc_leg, umin=[-5, -9.9], umax=[100,99]) - @test all((nmpc_leg.con.U0min, nmpc_leg.con.U0max) .≈ ([-5, -9.9], [100,99])) + @test nmpc_leg.con.U0min ≈ [-5, -9.9] + @test nmpc_leg.con.U0max ≈ [100,99] setconstraint!(nmpc_leg, Δumin=[-5,-10], Δumax=[6,11]) - @test all((nmpc_leg.con.ΔŨmin, nmpc_leg.con.ΔŨmax) .≈ ([-5,-10,0], [6,11,Inf])) + @test nmpc_leg.con.ΔŨmin ≈ [-5,-10,0] + @test nmpc_leg.con.ΔŨmax ≈ [6,11,Inf] setconstraint!(nmpc_leg, ymin=[-6, -11],ymax=[55, 35]) - @test all((nmpc_leg.con.Y0min, nmpc_leg.con.Y0max) .≈ ([-6,-11], [55,35])) + @test nmpc_leg.con.Y0min ≈ [-6,-11] + @test nmpc_leg.con.Y0max ≈ [55,35] setconstraint!(nmpc_leg, x̂min=[-21,-22,-23,-24,-25,-26], x̂max=[21,22,23,24,25,26]) - @test all((nmpc_leg.con.x̂0min, nmpc_leg.con.x̂0max) .≈ - ([-21,-22,-23,-24,-25,-26], [21,22,23,24,25,26])) + @test nmpc_leg.con.x̂0min ≈ [-21,-22,-23,-24,-25,-26] + @test nmpc_leg.con.x̂0max ≈ [21,22,23,24,25,26] setconstraint!(nmpc_leg, c_umin=[0.01,0.02], c_umax=[0.03,0.04]) - @test all((-nmpc_leg.con.A_Umin[:, end], -nmpc_leg.con.A_Umax[:, end]) .≈ - ([0.01,0.02], [0.03,0.04])) + @test -nmpc_leg.con.A_Umin[:, end] ≈ [0.01,0.02] + @test -nmpc_leg.con.A_Umax[:, end] ≈ [0.03,0.04] setconstraint!(nmpc_leg, c_Δumin=[0.05,0.06], c_Δumax=[0.07,0.08]) - @test all((-nmpc_leg.con.A_ΔŨmin[1:end-1, end], -nmpc_leg.con.A_ΔŨmax[1:end-1, end]) .≈ - ([0.05,0.06], [0.07,0.08])) + @test -nmpc_leg.con.A_ΔŨmin[1:end-1, end] ≈ [0.05,0.06] + @test -nmpc_leg.con.A_ΔŨmax[1:end-1, end] ≈ [0.07,0.08] setconstraint!(nmpc_leg, c_ymin=[1.00,1.01], c_ymax=[1.02,1.03]) - @test all((-nmpc_leg.con.A_Ymin, -nmpc_leg.con.A_Ymax) .≈ (zeros(0,3), zeros(0,3))) - @test all((nmpc_leg.con.C_ymin, nmpc_leg.con.C_ymax) .≈ ([1.00,1.01], [1.02,1.03])) + @test -nmpc_leg.con.A_Ymin ≈ zeros(0,3) + @test -nmpc_leg.con.A_Ymax ≈ zeros(0,3) + @test nmpc_leg.con.C_ymin ≈ [1.00,1.01] + @test nmpc_leg.con.C_ymax ≈ [1.02,1.03] setconstraint!(nmpc_leg, c_x̂min=[0.21,0.22,0.23,0.24,0.25,0.26], c_x̂max=[0.31,0.32,0.33,0.34,0.35,0.36] ) - @test all((nmpc_leg.con.c_x̂min, nmpc_leg.con.c_x̂max) .≈ - ([0.21,0.22,0.23,0.24,0.25,0.26], [0.31,0.32,0.33,0.34,0.35,0.36])) + @test nmpc_leg.con.c_x̂min ≈ [0.21,0.22,0.23,0.24,0.25,0.26] + @test nmpc_leg.con.c_x̂max ≈ [0.31,0.32,0.33,0.34,0.35,0.36] nmpc_ms = NonLinMPC(nonlinmodel, Hp=1, Hc=1, transcription=MultipleShooting()) - setconnmpc_ms.con.Y0min ≈ [-6,-11] + setconstraint!(nmpc_ms, ymin=[-6, -11],ymax=[55, 35]) + @test nmpc_ms.con.Y0min ≈ [-6,-11] @test nmpc_ms.con.Y0max ≈ [55,35] setconstraint!(nmpc_ms, x̂min=[-21,-22,-23,-24,-25,-26], x̂max=[21,22,23,24,25,26]) @test nmpc_ms.con.x̂0min ≈ [-21,-22,-23,-24,-25,-26] @@ -1145,10 +1179,6 @@ end @test -nmpc_lin.con.A_x̂max[:, end] ≈ [0.31,0.32,0.33,0.34,0.35,0.36] @test nmpc_ms.con.c_x̂min ≈ [0.21,0.22,0.23,0.24,0.25,0.26] @test nmpc_ms.con.c_x̂max ≈ [0.31,0.32,0.33,0.34,0.35,0.36] - @test -nmpc_lin.con.A_x̂min[:, end] ≈ [0.21,0.22,0.23,0.24,0.25,0.26] - @test -nmpc_lin.con.A_x̂max[:, end] ≈ [0.31,0.32,0.33,0.34,0.35,0.36] - @test nmpc_ms.con.c_x̂min ≈ [0.21,0.22,0.23,0.24,0.25,0.26] - @test nmpc_ms.con.c_x̂max ≈ [0.31,0.32,0.33,0.34,0.35,0.36] end From 60bfa6eee5ff29124af61fe331e2590e985a13d2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 16:32:28 -0500 Subject: [PATCH 28/43] doc: adding custom linear and nonlinear constraints in README --- README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index bbe274473..23bdc59f5 100644 --- a/README.md +++ b/README.md @@ -81,9 +81,10 @@ for more detailed examples. - 📸 **Linearization**: Auto-differentiation for exact Jacobians. - ⚙️ **Adaptive MPC**: Manual model updates or automatic successive linearization. - 🏎️ **Explicit MPC**: Specialized for unconstrained problems. -- 🚧 **Constraints**: Soft/hard limits on inputs, outputs, increments, and terminal states. +- 🚧 **Bounds**: Soft/hard limits on inputs, outputs, increments, and terminal states. +- 🚫 **Contraints**: Soft/hard custom linear and nonlinear inequality constraints. - 🔁 **Feedback**: Internal model or state estimators (see features below). -- 📡 **Feedforward**: Integrated support for measured disturbances. +- 📡 **Feedforward**: Integrated support for measured disturbances. - 🔮 **Preview**: Custom predictions for setpoints and measured disturbances. - 📈 **Offset-Free**: Automatic model augmentation with integrators. - 📊 **Visuals**: Easy integration with `Plots.jl`. From c5d691ae92b38e4147234fa969388660f575704e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 16:36:32 -0500 Subject: [PATCH 29/43] doc: `LinearMPC` in homepage. --- docs/src/index.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/docs/src/index.md b/docs/src/index.md index 3ded65373..8a9a8a851 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -14,7 +14,8 @@ real-time optimization. Modern MPCs based on closed-loop state estimators are th of the package, but classical approaches that rely on internal models are also possible. The `JuMP` and `DifferentiationInterface` dependencies allows the user to test different optimizers and automatic differentiation (AD) backends easily if the performances of the -default settings are not satisfactory. +default settings are not satisfactory. Linear MPC controllers can be exported to lightweight +and standalone C code via the `LinearMPC` package extension. The documentation is divided in two parts: From 6429c6e056a0548f3f5809e9afdbfd4f354acdb0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 16:46:36 -0500 Subject: [PATCH 30/43] debug: correctly modify custom linear constraint softness --- src/controller/construct.jl | 2 ++ test/3_test_predictive_control.jl | 7 +++++++ 2 files changed, 9 insertions(+) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 276ed0487..7b00ee2b7 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -445,6 +445,8 @@ function setconstraint!( isnothing(C_Δumax) && !isnothing(c_Δumax) && (C_Δumax = repeat(c_Δumax, Hc)) isnothing(C_ymin) && !isnothing(c_ymin) && (C_ymin = repeat(c_ymin, Hp)) isnothing(C_ymax) && !isnothing(c_ymax) && (C_ymax = repeat(c_ymax, Hp)) + isnothing(C_wmin) && !isnothing(c_wmin) && (C_wmin = repeat(c_wmin, Hp+1)) + isnothing(C_wmax) && !isnothing(c_wmax) && (C_wmax = repeat(c_wmax, Hp+1)) if !isnothing(C_umin) size(C_umin) == (nu*Hp,) || throw(ArgumentError("C_umin size must be $((nu*Hp,))")) any(<(0), C_umin) && error("C_umin weights should be non-negative") diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 4f1d8c16d..637b37745 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -295,6 +295,9 @@ end setconstraint!(mpc, ymin=[-6, -11],ymax=[55, 35]) @test mpc.con.Y0min ≈ [-6,-11] @test mpc.con.Y0max ≈ [55,35] + setconstraint!(mpc, wmin=[-7, -12], wmax=[75, 65]) + @test mpc.con.Wmin ≈ [-7, -12, -7, -12] + @test mpc.con.Wmax ≈ [75, 65, 75, 65] setconstraint!(mpc, x̂min=[-21,-22,-23,-24,-25,-26], x̂max=[21,22,23,24,25,26]) @test mpc.con.x̂0min ≈ [-21,-22,-23,-24,-25,-26] @test mpc.con.x̂0max ≈ [21,22,23,24,25,26] @@ -308,6 +311,9 @@ end setconstraint!(mpc, c_ymin=[1.00,1.01], c_ymax=[1.02,1.03]) @test -mpc.con.A_Ymin[:, end] ≈ [1.00,1.01] @test -mpc.con.A_Ymax[:, end] ≈ [1.02,1.03] + setconstraint!(mpc, c_wmin=[2.00,2.01], c_wmax=[2.02,2.03]) + @test -mpc.con.A_Wmin[:, end] ≈ [2.00,2.01,2.00,2.01] + @test -mpc.con.A_Wmax[:, end] ≈ [2.02,2.03,2.02,2.03] setconstraint!(mpc, c_x̂min=[0.21,0.22,0.23,0.24,0.25,0.26], c_x̂max=[0.31,0.32,0.33,0.34,0.35,0.36]) @test -mpc.con.A_x̂min[:, end] ≈ [0.21,0.22,0.23,0.24,0.25,0.26] @test -mpc.con.A_x̂max[:, end] ≈ [0.31,0.32,0.33,0.34,0.35,0.36] @@ -324,6 +330,7 @@ end setconstraint!(mpc2, Ymin=-1(1:50).-3, Ymax=+1(1:50).+3) @test mpc2.con.Y0min ≈ -1(1:50).-3 @test mpc2.con.Y0max ≈ +1(1:50).+3 + #setconstraint!(mpc3) setconstraint!(mpc2, C_umin=+1(1:50).+4, C_umax=+1(1:50).+4) @test -mpc2.con.A_Umin[:, end] ≈ +1(1:50).+4 From 4015f97e4b3a38c02054b89e593ed6a16f5e22a1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 17:24:26 -0500 Subject: [PATCH 31/43] changed: `DimensionMistmatch` instead of `ArgumentError` in `setconstraint!` --- src/controller/construct.jl | 56 +++++++++++++-------------- src/estimator/mhe/construct.jl | 36 +++++++++--------- test/2_test_state_estim.jl | 24 ++++++------ test/3_test_predictive_control.jl | 63 ++++++++++++++++++------------- 4 files changed, 94 insertions(+), 85 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 7b00ee2b7..8b32bac3b 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -349,84 +349,84 @@ function setconstraint!( nϵ, nw, nc = mpc.nϵ, con.nw, con.nc notSolvedYet = (JuMP.termination_status(optim) == JuMP.OPTIMIZE_NOT_CALLED) if isnothing(Umin) && !isnothing(umin) - size(umin) == (nu,) || throw(ArgumentError("umin size must be $((nu,))")) + size(umin) == (nu,) || throw(DimensionMismatch("umin size must be $((nu,))")) for i = 1:nu*Hp con.U0min[i] = umin[(i-1) % nu + 1] - mpc.Uop[i] end elseif !isnothing(Umin) - size(Umin) == (nu*Hp,) || throw(ArgumentError("Umin size must be $((nu*Hp,))")) + size(Umin) == (nu*Hp,) || throw(DimensionMismatch("Umin size must be $((nu*Hp,))")) con.U0min .= Umin .- mpc.Uop end if isnothing(Umax) && !isnothing(umax) - size(umax) == (nu,) || throw(ArgumentError("umax size must be $((nu,))")) + size(umax) == (nu,) || throw(DimensionMismatch("umax size must be $((nu,))")) for i = 1:nu*Hp con.U0max[i] = umax[(i-1) % nu + 1] - mpc.Uop[i] end elseif !isnothing(Umax) - size(Umax) == (nu*Hp,) || throw(ArgumentError("Umax size must be $((nu*Hp,))")) + size(Umax) == (nu*Hp,) || throw(DimensionMismatch("Umax size must be $((nu*Hp,))")) con.U0max .= Umax .- mpc.Uop end if isnothing(ΔUmin) && !isnothing(Δumin) - size(Δumin) == (nu,) || throw(ArgumentError("Δumin size must be $((nu,))")) + size(Δumin) == (nu,) || throw(DimensionMismatch("Δumin size must be $((nu,))")) for i = 1:nu*Hc con.ΔŨmin[i] = Δumin[(i-1) % nu + 1] end elseif !isnothing(ΔUmin) - size(ΔUmin) == (nu*Hc,) || throw(ArgumentError("ΔUmin size must be $((nu*Hc,))")) + size(ΔUmin) == (nu*Hc,) || throw(DimensionMismatch("ΔUmin size must be $((nu*Hc,))")) con.ΔŨmin[1:nu*Hc] .= ΔUmin end if isnothing(ΔUmax) && !isnothing(Δumax) - size(Δumax) == (nu,) || throw(ArgumentError("Δumax size must be $((nu,))")) + size(Δumax) == (nu,) || throw(DimensionMismatch("Δumax size must be $((nu,))")) for i = 1:nu*Hc con.ΔŨmax[i] = Δumax[(i-1) % nu + 1] end elseif !isnothing(ΔUmax) - size(ΔUmax) == (nu*Hc,) || throw(ArgumentError("ΔUmax size must be $((nu*Hc,))")) + size(ΔUmax) == (nu*Hc,) || throw(DimensionMismatch("ΔUmax size must be $((nu*Hc,))")) con.ΔŨmax[1:nu*Hc] .= ΔUmax end if isnothing(Ymin) && !isnothing(ymin) - size(ymin) == (ny,) || throw(ArgumentError("ymin size must be $((ny,))")) + size(ymin) == (ny,) || throw(DimensionMismatch("ymin size must be $((ny,))")) for i = 1:ny*Hp con.Y0min[i] = ymin[(i-1) % ny + 1] - mpc.Yop[i] end elseif !isnothing(Ymin) - size(Ymin) == (ny*Hp,) || throw(ArgumentError("Ymin size must be $((ny*Hp,))")) + size(Ymin) == (ny*Hp,) || throw(DimensionMismatch("Ymin size must be $((ny*Hp,))")) con.Y0min .= Ymin .- mpc.Yop end if isnothing(Ymax) && !isnothing(ymax) - size(ymax) == (ny,) || throw(ArgumentError("ymax size must be $((ny,))")) + size(ymax) == (ny,) || throw(DimensionMismatch("ymax size must be $((ny,))")) for i = 1:ny*Hp con.Y0max[i] = ymax[(i-1) % ny + 1] - mpc.Yop[i] end elseif !isnothing(Ymax) - size(Ymax) == (ny*Hp,) || throw(ArgumentError("Ymax size must be $((ny*Hp,))")) + size(Ymax) == (ny*Hp,) || throw(DimensionMismatch("Ymax size must be $((ny*Hp,))")) con.Y0max .= Ymax .- mpc.Yop end if isnothing(Wmin) && !isnothing(wmin) - size(wmin) == (nw,) || throw(ArgumentError("wmin size must be $((nw,))")) + size(wmin) == (nw,) || throw(DimensionMismatch("wmin size must be $((nw,))")) for i = 1:nw*(Hp+1) con.Wmin[i] = wmin[(i-1) % nw + 1] end elseif !isnothing(Wmin) - size(Wmin) == (nw*(Hp+1),) || throw(ArgumentError("Wmin size must be $((nw*(Hp+1),))")) + size(Wmin) == (nw*(Hp+1),) || throw(DimensionMismatch("Wmin size must be $((nw*(Hp+1),))")) con.Wmin .= Wmin end if isnothing(Wmax) && !isnothing(wmax) - size(wmax) == (nw,) || throw(ArgumentError("wmax size must be $((nw,))")) + size(wmax) == (nw,) || throw(DimensionMismatch("wmax size must be $((nw,))")) for i = 1:nw*(Hp+1) con.Wmax[i] = wmax[(i-1) % nw + 1] end elseif !isnothing(Wmax) - size(Wmax) == (nw*(Hp+1),) || throw(ArgumentError("Wmax size must be $((nw*(Hp+1),))")) + size(Wmax) == (nw*(Hp+1),) || throw(DimensionMismatch("Wmax size must be $((nw*(Hp+1),))")) con.Wmax .= Wmax end if !isnothing(x̂min) - size(x̂min) == (nx̂,) || throw(ArgumentError("x̂min size must be $((nx̂,))")) + size(x̂min) == (nx̂,) || throw(DimensionMismatch("x̂min size must be $((nx̂,))")) con.x̂0min .= x̂min .- mpc.estim.x̂op end if !isnothing(x̂max) - size(x̂max) == (nx̂,) || throw(ArgumentError("x̂max size must be $((nx̂,))")) + size(x̂max) == (nx̂,) || throw(DimensionMismatch("x̂max size must be $((nx̂,))")) con.x̂0max .= x̂max .- mpc.estim.x̂op end allECRs = ( @@ -448,57 +448,57 @@ function setconstraint!( isnothing(C_wmin) && !isnothing(c_wmin) && (C_wmin = repeat(c_wmin, Hp+1)) isnothing(C_wmax) && !isnothing(c_wmax) && (C_wmax = repeat(c_wmax, Hp+1)) if !isnothing(C_umin) - size(C_umin) == (nu*Hp,) || throw(ArgumentError("C_umin size must be $((nu*Hp,))")) + size(C_umin) == (nu*Hp,) || throw(DimensionMismatch("C_umin size must be $((nu*Hp,))")) any(<(0), C_umin) && error("C_umin weights should be non-negative") con.A_Umin[:, end] .= -C_umin end if !isnothing(C_umax) - size(C_umax) == (nu*Hp,) || throw(ArgumentError("C_umax size must be $((nu*Hp,))")) + size(C_umax) == (nu*Hp,) || throw(DimensionMismatch("C_umax size must be $((nu*Hp,))")) any(<(0), C_umax) && error("C_umax weights should be non-negative") con.A_Umax[:, end] .= -C_umax end if !isnothing(C_Δumin) - size(C_Δumin) == (nu*Hc,) || throw(ArgumentError("C_Δumin size must be $((nu*Hc,))")) + size(C_Δumin) == (nu*Hc,) || throw(DimensionMismatch("C_Δumin size must be $((nu*Hc,))")) any(<(0), C_Δumin) && error("C_Δumin weights should be non-negative") con.A_ΔŨmin[1:end-1, end] .= -C_Δumin end if !isnothing(C_Δumax) - size(C_Δumax) == (nu*Hc,) || throw(ArgumentError("C_Δumax size must be $((nu*Hc,))")) + size(C_Δumax) == (nu*Hc,) || throw(DimensionMismatch("C_Δumax size must be $((nu*Hc,))")) any(<(0), C_Δumax) && error("C_Δumax weights should be non-negative") con.A_ΔŨmax[1:end-1, end] .= -C_Δumax end if !isnothing(C_ymin) - size(C_ymin) == (ny*Hp,) || throw(ArgumentError("C_ymin size must be $((ny*Hp,))")) + size(C_ymin) == (ny*Hp,) || throw(DimensionMismatch("C_ymin size must be $((ny*Hp,))")) any(<(0), C_ymin) && error("C_ymin weights should be non-negative") con.C_ymin .= C_ymin size(con.A_Ymin, 1) ≠ 0 && (con.A_Ymin[:, end] .= -con.C_ymin) # for LinModel end if !isnothing(C_ymax) - size(C_ymax) == (ny*Hp,) || throw(ArgumentError("C_ymax size must be $((ny*Hp,))")) + size(C_ymax) == (ny*Hp,) || throw(DimensionMismatch("C_ymax size must be $((ny*Hp,))")) any(<(0), C_ymax) && error("C_ymax weights should be non-negative") con.C_ymax .= C_ymax size(con.A_Ymax, 1) ≠ 0 && (con.A_Ymax[:, end] .= -con.C_ymax) # for LinModel end if !isnothing(C_wmin) - size(C_wmin) == (nw*(Hp+1),) || throw(ArgumentError("C_wmin size must be $((nw*(Hp+1),))")) + size(C_wmin) == (nw*(Hp+1),) || throw(DimensionMismatch("C_wmin size must be $((nw*(Hp+1),))")) any(<(0), C_wmin) && error("C_wmin weights should be non-negative") con.C_wmin .= C_wmin con.A_Wmin[:, end] .= -C_wmin end if !isnothing(C_wmax) - size(C_wmax) == (nw*(Hp+1),) || throw(ArgumentError("C_wmax size must be $((nw*(Hp+1),))")) + size(C_wmax) == (nw*(Hp+1),) || throw(DimensionMismatch("C_wmax size must be $((nw*(Hp+1),))")) any(<(0), C_wmax) && error("C_wmax weights should be non-negative") con.C_wmax .= C_wmax con.A_Wmax[:, end] .= -C_wmax end if !isnothing(c_x̂min) - size(c_x̂min) == (nx̂,) || throw(ArgumentError("c_x̂min size must be $((nx̂,))")) + size(c_x̂min) == (nx̂,) || throw(DimensionMismatch("c_x̂min size must be $((nx̂,))")) any(<(0), c_x̂min) && error("c_x̂min weights should be non-negative") con.c_x̂min .= c_x̂min size(con.A_x̂min, 1) ≠ 0 && (con.A_x̂min[:, end] .= -con.c_x̂min) # for LinModel end if !isnothing(c_x̂max) - size(c_x̂max) == (nx̂,) || throw(ArgumentError("c_x̂max size must be $((nx̂,))")) + size(c_x̂max) == (nx̂,) || throw(DimensionMismatch("c_x̂max size must be $((nx̂,))")) any(<(0), c_x̂max) && error("c_x̂max weights should be non-negative") con.c_x̂max .= c_x̂max size(con.A_x̂max, 1) ≠ 0 && (con.A_x̂max[:, end] .= -con.c_x̂max) # for LinModel diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index eb93d4baa..a29a08d85 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -621,61 +621,61 @@ function setconstraint!( notSolvedYet = (JuMP.termination_status(optim) == JuMP.OPTIMIZE_NOT_CALLED) C = estim.C if isnothing(X̂min) && !isnothing(x̂min) - size(x̂min) == (nx̂,) || throw(ArgumentError("x̂min size must be $((nx̂,))")) + size(x̂min) == (nx̂,) || throw(DimensionMismatch("x̂min size must be $((nx̂,))")) con.x̃0min[end-nx̂+1:end] .= x̂min .- estim.x̂op # if C is finite : x̃ = [ε; x̂] for i in 1:nx̂*He con.X̂0min[i] = x̂min[(i-1) % nx̂ + 1] - estim.X̂op[i] end elseif !isnothing(X̂min) - size(X̂min) == (nX̂con,) || throw(ArgumentError("X̂min size must be $((nX̂con,))")) + size(X̂min) == (nX̂con,) || throw(DimensionMismatch("X̂min size must be $((nX̂con,))")) con.x̃0min[end-nx̂+1:end] .= X̂min[1:nx̂] .- estim.x̂op con.X̂0min .= @views X̂min[nx̂+1:end] .- estim.X̂op end if isnothing(X̂max) && !isnothing(x̂max) - size(x̂max) == (nx̂,) || throw(ArgumentError("x̂max size must be $((nx̂,))")) + size(x̂max) == (nx̂,) || throw(DimensionMismatch("x̂max size must be $((nx̂,))")) con.x̃0max[end-nx̂+1:end] .= x̂max .- estim.x̂op # if C is finite : x̃ = [ε; x̂] for i in 1:nx̂*He con.X̂0max[i] = x̂max[(i-1) % nx̂ + 1] - estim.X̂op[i] end elseif !isnothing(X̂max) - size(X̂max) == (nX̂con,) || throw(ArgumentError("X̂max size must be $((nX̂con,))")) + size(X̂max) == (nX̂con,) || throw(DimensionMismatch("X̂max size must be $((nX̂con,))")) con.x̃0max[end-nx̂+1:end] .= X̂max[1:nx̂] .- estim.x̂op con.X̂0max .= @views X̂max[nx̂+1:end] .- estim.X̂op end if isnothing(Ŵmin) && !isnothing(ŵmin) - size(ŵmin) == (nŵ,) || throw(ArgumentError("ŵmin size must be $((nŵ,))")) + size(ŵmin) == (nŵ,) || throw(DimensionMismatch("ŵmin size must be $((nŵ,))")) for i in 1:nŵ*He con.Ŵmin[i] = ŵmin[(i-1) % nŵ + 1] end elseif !isnothing(Ŵmin) - size(Ŵmin) == (nŵ*He,) || throw(ArgumentError("Ŵmin size must be $((nŵ*He,))")) + size(Ŵmin) == (nŵ*He,) || throw(DimensionMismatch("Ŵmin size must be $((nŵ*He,))")) con.Ŵmin .= Ŵmin end if isnothing(Ŵmax) && !isnothing(ŵmax) - size(ŵmax) == (nŵ,) || throw(ArgumentError("ŵmax size must be $((nŵ,))")) + size(ŵmax) == (nŵ,) || throw(DimensionMismatch("ŵmax size must be $((nŵ,))")) for i in 1:nŵ*He con.Ŵmax[i] = ŵmax[(i-1) % nŵ + 1] end elseif !isnothing(Ŵmax) - size(Ŵmax) == (nŵ*He,) || throw(ArgumentError("Ŵmax size must be $((nŵ*He,))")) + size(Ŵmax) == (nŵ*He,) || throw(DimensionMismatch("Ŵmax size must be $((nŵ*He,))")) con.Ŵmax .= Ŵmax end if isnothing(V̂min) && !isnothing(v̂min) - size(v̂min) == (nym,) || throw(ArgumentError("v̂min size must be $((nym,))")) + size(v̂min) == (nym,) || throw(DimensionMismatch("v̂min size must be $((nym,))")) for i in 1:nym*He con.V̂min[i] = v̂min[(i-1) % nym + 1] end elseif !isnothing(V̂min) - size(V̂min) == (nym*He,) || throw(ArgumentError("V̂min size must be $((nym*He,))")) + size(V̂min) == (nym*He,) || throw(DimensionMismatch("V̂min size must be $((nym*He,))")) con.V̂min .= V̂min end if isnothing(V̂max) && !isnothing(v̂max) - size(v̂max) == (nym,) || throw(ArgumentError("v̂max size must be $((nym,))")) + size(v̂max) == (nym,) || throw(DimensionMismatch("v̂max size must be $((nym,))")) for i in 1:nym*He con.V̂max[i] = v̂max[(i-1) % nym + 1] end elseif !isnothing(V̂max) - size(V̂max) == (nym*He,) || throw(ArgumentError("V̂max size must be $((nym*He,))")) + size(V̂max) == (nym*He,) || throw(DimensionMismatch("V̂max size must be $((nym*He,))")) con.V̂max .= V̂max end allECRs = ( @@ -694,7 +694,7 @@ function setconstraint!( isnothing(C_v̂min) && !isnothing(c_v̂min) && (C_v̂min = repeat(c_v̂min, He)) isnothing(C_v̂max) && !isnothing(c_v̂max) && (C_v̂max = repeat(c_v̂max, He)) if !isnothing(C_x̂min) - size(C_x̂min) == (nX̂con,) || throw(ArgumentError("C_x̂min size must be $((nX̂con,))")) + size(C_x̂min) == (nX̂con,) || throw(DimensionMismatch("C_x̂min size must be $((nX̂con,))")) any(C_x̂min .< 0) && error("C_x̂min weights should be non-negative") # if C is finite : x̃ = [ε; x̂] con.A_x̃min[end-nx̂+1:end, end] .= @views -C_x̂min[1:nx̂] @@ -702,7 +702,7 @@ function setconstraint!( size(con.A_X̂min, 1) ≠ 0 && (con.A_X̂min[:, end] = -con.C_x̂min) # for LinModel end if !isnothing(C_x̂max) - size(C_x̂max) == (nX̂con,) || throw(ArgumentError("C_x̂max size must be $((nX̂con,))")) + size(C_x̂max) == (nX̂con,) || throw(DimensionMismatch("C_x̂max size must be $((nX̂con,))")) any(C_x̂max .< 0) && error("C_x̂max weights should be non-negative") # if C is finite : x̃ = [ε; x̂] : con.A_x̃max[end-nx̂+1:end, end] .= @views -C_x̂max[1:nx̂] @@ -710,23 +710,23 @@ function setconstraint!( size(con.A_X̂max, 1) ≠ 0 && (con.A_X̂max[:, end] = -con.C_x̂max) # for LinModel end if !isnothing(C_ŵmin) - size(C_ŵmin) == (nŵ*He,) || throw(ArgumentError("C_ŵmin size must be $((nŵ*He,))")) + size(C_ŵmin) == (nŵ*He,) || throw(DimensionMismatch("C_ŵmin size must be $((nŵ*He,))")) any(C_ŵmin .< 0) && error("C_ŵmin weights should be non-negative") con.A_Ŵmin[:, end] .= -C_ŵmin end if !isnothing(C_ŵmax) - size(C_ŵmax) == (nŵ*He,) || throw(ArgumentError("C_ŵmax size must be $((nŵ*He,))")) + size(C_ŵmax) == (nŵ*He,) || throw(DimensionMismatch("C_ŵmax size must be $((nŵ*He,))")) any(C_ŵmax .< 0) && error("C_ŵmax weights should be non-negative") con.A_Ŵmax[:, end] .= -C_ŵmax end if !isnothing(C_v̂min) - size(C_v̂min) == (nym*He,) || throw(ArgumentError("C_v̂min size must be $((nym*He,))")) + size(C_v̂min) == (nym*He,) || throw(DimensionMismatch("C_v̂min size must be $((nym*He,))")) any(C_v̂min .< 0) && error("C_v̂min weights should be non-negative") con.C_v̂min .= C_v̂min size(con.A_V̂min, 1) ≠ 0 && (con.A_V̂min[:, end] = -con.C_v̂min) # for LinModel end if !isnothing(C_v̂max) - size(C_v̂max) == (nym*He,) || throw(ArgumentError("C_v̂max size must be $((nym*He,))")) + size(C_v̂max) == (nym*He,) || throw(DimensionMismatch("C_v̂max size must be $((nym*He,))")) any(C_v̂max .< 0) && error("C_v̂max weights should be non-negative") con.C_v̂max .= C_v̂max size(con.A_V̂max, 1) ≠ 0 && (con.A_V̂max[:, end] = -con.C_v̂max) # for LinModel diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 3fff7327b..fb671a930 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1231,18 +1231,18 @@ end @test mhe3.con.C_v̂min ≈ 0.03(11:18) @test mhe3.con.C_v̂max ≈ 0.04(11:18) - @test_throws ArgumentError setconstraint!(mhe2, x̂min=[-1]) - @test_throws ArgumentError setconstraint!(mhe2, x̂max=[+1]) - @test_throws ArgumentError setconstraint!(mhe2, ŵmin=[-1]) - @test_throws ArgumentError setconstraint!(mhe2, ŵmax=[+1]) - @test_throws ArgumentError setconstraint!(mhe2, v̂min=[-1]) - @test_throws ArgumentError setconstraint!(mhe2, v̂max=[+1]) - @test_throws ArgumentError setconstraint!(mhe2, c_x̂min=[-1]) - @test_throws ArgumentError setconstraint!(mhe2, c_x̂max=[+1]) - @test_throws ArgumentError setconstraint!(mhe2, c_ŵmin=[-1]) - @test_throws ArgumentError setconstraint!(mhe2, c_ŵmax=[+1]) - @test_throws ArgumentError setconstraint!(mhe2, c_v̂min=[-1]) - @test_throws ArgumentError setconstraint!(mhe2, c_v̂max=[+1]) + @test_throws DimensionMismatch setconstraint!(mhe2, x̂min=[-1]) + @test_throws DimensionMismatch setconstraint!(mhe2, x̂max=[+1]) + @test_throws DimensionMismatch setconstraint!(mhe2, ŵmin=[-1]) + @test_throws DimensionMismatch setconstraint!(mhe2, ŵmax=[+1]) + @test_throws DimensionMismatch setconstraint!(mhe2, v̂min=[-1]) + @test_throws DimensionMismatch setconstraint!(mhe2, v̂max=[+1]) + @test_throws DimensionMismatch setconstraint!(mhe2, c_x̂min=[+2]) + @test_throws DimensionMismatch setconstraint!(mhe2, c_x̂max=[+1]) + @test_throws DimensionMismatch setconstraint!(mhe2, c_ŵmin=[+2]) + @test_throws DimensionMismatch setconstraint!(mhe2, c_ŵmax=[+1]) + @test_throws DimensionMismatch setconstraint!(mhe2, c_v̂min=[+2]) + @test_throws DimensionMismatch setconstraint!(mhe2, c_v̂max=[+1]) preparestate!(mhe1, [50, 30]) updatestate!(mhe1, [10, 50], [50, 30]) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 637b37745..78587884e 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -319,7 +319,7 @@ end @test -mpc.con.A_x̂max[:, end] ≈ [0.31,0.32,0.33,0.34,0.35,0.36] model2 = LinModel(tf([2], [10, 1]), 3.0) - mpc2 = LinMPC(model2, Hp=50, Hc=5) + mpc2 = LinMPC(model2, Hp=50, Hc=5, Wr=[1]) setconstraint!(mpc2, Umin=-1(1:50).-1, Umax=+1(1:50).+1) @test mpc2.con.U0min ≈ -1(1:50).-1 @@ -330,31 +330,40 @@ end setconstraint!(mpc2, Ymin=-1(1:50).-3, Ymax=+1(1:50).+3) @test mpc2.con.Y0min ≈ -1(1:50).-3 @test mpc2.con.Y0max ≈ +1(1:50).+3 - #setconstraint!(mpc3) - - setconstraint!(mpc2, C_umin=+1(1:50).+4, C_umax=+1(1:50).+4) - @test -mpc2.con.A_Umin[:, end] ≈ +1(1:50).+4 - @test -mpc2.con.A_Umax[:, end] ≈ +1(1:50).+4 - setconstraint!(mpc2, C_Δumin=+1(1:5).+5, C_Δumax=+1(1:5).+5) - @test -mpc2.con.A_ΔŨmin[1:end-1, end] ≈ +1(1:5).+5 - @test -mpc2.con.A_ΔŨmax[1:end-1, end] ≈ +1(1:5).+5 - setconstraint!(mpc2, C_ymin=+1(1:50).+6, C_ymax=+1(1:50).+6) - @test -mpc2.con.A_Ymin[:, end] ≈ +1(1:50).+6 - @test -mpc2.con.A_Ymax[:, end] ≈ +1(1:50).+6 - setconstraint!(mpc2, c_umin=[0], c_umax=[0], c_Δumin=[0], c_Δumax=[0], c_ymin=[1], c_ymax=[1]) - - @test_throws ArgumentError setconstraint!(mpc, umin=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, umax=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, Δumin=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, Δumax=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, ymin=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, ymax=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, c_umin=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, c_umax=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, c_Δumin=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, c_Δumax=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, c_ymin=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, c_ymax=[0,0,0]) + setconstraint!(mpc2, Wmin=-1(1:51).-4, Wmax=+1(1:51).+4) + @test mpc2.con.Wmin ≈ -1(1:51).-4 + @test mpc2.con.Wmax ≈ +1(1:51).+4 + + setconstraint!(mpc2, C_umin=+1(1:50).+5, C_umax=+1(1:50).+5) + @test -mpc2.con.A_Umin[:, end] ≈ +1(1:50).+5 + @test -mpc2.con.A_Umax[:, end] ≈ +1(1:50).+5 + setconstraint!(mpc2, C_Δumin=+1(1:5).+6, C_Δumax=+1(1:5).+6) + @test -mpc2.con.A_ΔŨmin[1:end-1, end] ≈ +1(1:5).+6 + @test -mpc2.con.A_ΔŨmax[1:end-1, end] ≈ +1(1:5).+6 + setconstraint!(mpc2, C_ymin=+1(1:50).+7, C_ymax=+1(1:50).+7) + @test -mpc2.con.A_Ymin[:, end] ≈ +1(1:50).+7 + @test -mpc2.con.A_Ymax[:, end] ≈ +1(1:50).+7 + setconstraint!(mpc2, C_wmin=+1(1:51).+8, C_wmax=+1(1:51).+8) + @test -mpc2.con.A_Wmin[:, end] ≈ +1(1:51).+8 + @test -mpc2.con.A_Wmax[:, end] ≈ +1(1:51).+8 + + setconstraint!(mpc2, + c_umin=[0], c_umax=[0], c_Δumin=[0], c_Δumax=[0], + c_ymin=[1], c_ymax=[1], c_wmin=[1], c_wmax=[1] + ) + + @test_throws DimensionMismatch setconstraint!(mpc, umin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, umax=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, Δumin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, Δumax=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, ymin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, ymax=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, c_umin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, c_umax=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, c_Δumin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, c_Δumax=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, c_ymin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, c_ymax=[0,0,0]) preparestate!(mpc, mpc.estim.model.yop, mpc.estim.model.dop) moveinput!(mpc, [0, 0], [0]) @@ -806,7 +815,7 @@ end @test isa(nmpc15.estim, UnscentedKalmanFilter{Float32}) @test isa(nmpc15.optim, JuMP.GenericModel{Float64}) # Ipopt does not support Float32 - @test_throws ArgumentError NonLinMPC(nonlinmodel, Hp=15, Ewt=[1, 1]) + @test_throws DimensionMismatch NonLinMPC(nonlinmodel, Hp=15, Ewt=[1, 1]) @test_throws ArgumentError NonLinMPC(nonlinmodel) @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, JE = (_,_,_)->0.0) @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, gc = (_,_,_,_)->[0.0], nc=1) From 0e7fa0900cbbe98125a6809c78f0a99310673a11 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Feb 2026 17:44:02 -0500 Subject: [PATCH 32/43] changed: `DimensionMismatch` except. for all kalman Filters --- src/estimator/construct.jl | 13 ++++++++----- test/2_test_state_estim.jl | 22 +++++++++++----------- 2 files changed, 19 insertions(+), 16 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 897d06c9b..c6e0d2219 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -122,14 +122,16 @@ Also validate initial estimate covariance `P̂_0`, if provided. function validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0=nothing) nym = length(i_ym) nx̂ = model.nx + sum(nint_u) + sum(nint_ym) - size(Q̂) ≠ (nx̂, nx̂) && error("Q̂ size $(size(Q̂)) ≠ nx̂, nx̂ $((nx̂, nx̂))") + (any(<(0), nint_u) || any(<(0), nint_ym)) && return nothing # a clearer error is thrown later + size(Q̂) ≠ (nx̂, nx̂) && throw(DimensionMismatch("Q̂ size $(size(Q̂)) ≠ nx̂, nx̂ $((nx̂, nx̂))")) !ishermitian(Q̂) && error("Q̂ is not Hermitian") - size(R̂) ≠ (nym, nym) && error("R̂ size $(size(R̂)) ≠ nym, nym $((nym, nym))") + size(R̂) ≠ (nym, nym) && throw(DimensionMismatch("R̂ size $(size(R̂)) ≠ nym, nym $((nym, nym))")) !ishermitian(R̂) && error("R̂ is not Hermitian") if ~isnothing(P̂_0) - size(P̂_0) ≠ (nx̂, nx̂) && error("P̂_0 size $(size(P̂_0)) ≠ nx̂, nx̂ $((nx̂, nx̂))") + size(P̂_0) ≠ (nx̂, nx̂) && throw(DimensionMismatch("P̂_0 size $(size(P̂_0)) ≠ nx̂, nx̂ $((nx̂, nx̂))")) !ishermitian(P̂_0) && error("P̂_0 is not Hermitian") end + return nothing end @doc raw""" @@ -210,9 +212,10 @@ function init_integrators(nint::IntVectorOrInt, ny, varname::String) nint = fill(0, ny) end if length(nint) ≠ ny - error("nint_$(varname) length ($(length(nint))) ≠ n$(varname) ($ny)") + msg = "nint_$(varname) length ($(length(nint))) ≠ n$(varname) ($ny)" + throw(DimensionMismatch(msg)) end - any(nint .< 0) && error("nint_$(varname) values should be ≥ 0") + any(nint .< 0) && throw(ArgumentError("nint_$(varname) values should be ≥ 0")) nx = sum(nint) A, C = zeros(nx, nx), zeros(ny, nx) if nx ≠ 0 diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index fb671a930..0c55da08c 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -49,10 +49,10 @@ @test skalmanfilter9.cov.Q̂ ≈ I(4) @test skalmanfilter9.cov.R̂ ≈ I(2) - @test_throws ErrorException SteadyKalmanFilter(linmodel, nint_ym=[1,1,1]) - @test_throws ErrorException SteadyKalmanFilter(linmodel, nint_ym=[-1,0]) - @test_throws ErrorException SteadyKalmanFilter(linmodel, nint_ym=0, σQ=[1]) - @test_throws ErrorException SteadyKalmanFilter(linmodel, nint_ym=0, σR=[1,1,1]) + @test_throws DimensionMismatch SteadyKalmanFilter(linmodel, nint_ym=[1,1,1]) + @test_throws ArgumentError SteadyKalmanFilter(linmodel, nint_ym=[-1,0]) + @test_throws DimensionMismatch SteadyKalmanFilter(linmodel, nint_ym=0, σQ=[1]) + @test_throws DimensionMismatch SteadyKalmanFilter(linmodel, nint_ym=0, σR=[1,1,1]) @test_throws ErrorException SteadyKalmanFilter(linmodel3, nint_ym=[1, 0, 0]) model_unobs = LinModel([1 0;0 1.5], [1; 0], [1 0], zeros(2,0), zeros(1,0), 1.0) @test_throws ErrorException SteadyKalmanFilter(model_unobs, nint_ym=[1]) @@ -188,7 +188,7 @@ end kalmanfilter8 = KalmanFilter(linmodel2) @test isa(kalmanfilter8, KalmanFilter{Float32}) - @test_throws ErrorException KalmanFilter(linmodel, nint_ym=0, σP_0=[1]) + @test_throws DimensionMismatch KalmanFilter(linmodel, nint_ym=0, σP_0=[1]) end @testitem "KalmanFilter estimator methods" setup=[SetupMPCtests] begin @@ -302,8 +302,8 @@ end lo6 = Luenberger(linmodel2) @test isa(lo6, Luenberger{Float32}) - @test_throws ErrorException Luenberger(linmodel, nint_ym=[1,1,1]) - @test_throws ErrorException Luenberger(linmodel, nint_ym=[-1,0]) + @test_throws DimensionMismatch Luenberger(linmodel, nint_ym=[1,1,1]) + @test_throws ArgumentError Luenberger(linmodel, nint_ym=[-1,0]) @test_throws ErrorException Luenberger(linmodel, poles=[0.5]) @test_throws ErrorException Luenberger(linmodel, poles=fill(1.5, lo1.nx̂)) @test_throws ErrorException Luenberger(LinModel(tf(1,[1, 0]),0.1), poles=[0.5,0.6]) @@ -1603,8 +1603,8 @@ end @testitem "ManualEstimator construction" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(sys,Ts,i_u=[1,2]) - f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d - h(x,d,model) = model.C*x + model.Du*d + f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d + h = (x,d,model) -> model.C*x + model.Du*d nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel) manual1 = ManualEstimator(linmodel) @@ -1651,8 +1651,8 @@ end @testitem "ManualEstimator estimator methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(sys,Ts,i_u=[1,2]) - f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d - h(x,d,model) = model.C*x + model.Du*d + f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d + h = (x,d,model) -> model.C*x + model.Du*d nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, 0, solver=nothing, p=linmodel) manual1 = ManualEstimator(linmodel) From 47c908dacae839e203b7e468a7a7d5e24bb31981 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 10:06:27 -0500 Subject: [PATCH 33/43] test: `LinMPC` and custom constraint violation --- test/3_test_predictive_control.jl | 37 ++++++++++++++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 78587884e..ac4465b58 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -358,13 +358,25 @@ end @test_throws DimensionMismatch setconstraint!(mpc, Δumax=[0,0,0]) @test_throws DimensionMismatch setconstraint!(mpc, ymin=[0,0,0]) @test_throws DimensionMismatch setconstraint!(mpc, ymax=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, wmin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, wmax=[0,0,0]) @test_throws DimensionMismatch setconstraint!(mpc, c_umin=[0,0,0]) @test_throws DimensionMismatch setconstraint!(mpc, c_umax=[0,0,0]) @test_throws DimensionMismatch setconstraint!(mpc, c_Δumin=[0,0,0]) @test_throws DimensionMismatch setconstraint!(mpc, c_Δumax=[0,0,0]) @test_throws DimensionMismatch setconstraint!(mpc, c_ymin=[0,0,0]) @test_throws DimensionMismatch setconstraint!(mpc, c_ymax=[0,0,0]) - + @test_throws DimensionMismatch setconstraint!(mpc, c_wmin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, c_wmax=[0,0,0]) + @test_throws ErrorException setconstraint!(mpc, c_umin=[-1,-1]) + @test_throws ErrorException setconstraint!(mpc, c_umax=[-1,-1]) + @test_throws ErrorException setconstraint!(mpc, c_Δumin=[-1,-1]) + @test_throws ErrorException setconstraint!(mpc, c_Δumax=[-1,-1]) + @test_throws ErrorException setconstraint!(mpc, c_ymin=[-1,-1]) + @test_throws ErrorException setconstraint!(mpc, c_ymax=[-1,-1]) + @test_throws ErrorException setconstraint!(mpc, c_wmin=[-1,-1]) + @test_throws ErrorException setconstraint!(mpc, c_wmax=[-1,-1]) + preparestate!(mpc, mpc.estim.model.yop, mpc.estim.model.dop) moveinput!(mpc, [0, 0], [0]) @test_throws ErrorException setconstraint!(mpc, c_umin=[1, 1], c_umax=[1, 1]) @@ -436,6 +448,29 @@ end info = getinfo(mpc) @test info[:x̂end][1] ≈ 0 atol=1e-1 setconstraint!(mpc, x̂min=[-1e6,-Inf], x̂max=[+1e6,+Inf]) + + model2 = LinModel([tf([2], [10, 1]) tf(0.1, [7, 1])], 3.0, i_d=[2]) + model2 = setop!(model2, uop=[25], dop=[30], yop=[50]) + mpc_wy = LinMPC(model2, Nwt=[0], Cwt=Inf, Hp=50, Hc=50, Wy=[1]) + mpc_wy = setconstraint!(mpc_wy, wmax=[75]) + preparestate!(mpc_wy, [50], [30]) + u = moveinput!(mpc_wy, [100], [30]) + @test all(isapprox.(getinfo(mpc_wy)[:Ŷ], 75.0; atol=1e-1)) + mpc_wu = LinMPC(model2, Nwt=[0], Cwt=Inf, Hp=50, Hc=50, Wu=[1]) + mpc_wu = setconstraint!(mpc_wu, wmax=[20]) + preparestate!(mpc_wu, [50], [30]) + u = moveinput!(mpc_wu, [100], [30]) + @test all(isapprox.(getinfo(mpc_wu)[:U], 20.0; atol=1e-1)) + mpc_wd = LinMPC(model2, Nwt=[0], Cwt=Inf, Hp=50, Hc=50, Wd=[1], Wy=[1]) + mpc_wd = setconstraint!(mpc_wd, wmax=[95]) + preparestate!(mpc_wd, [50], [30]) + u = moveinput!(mpc_wd, [100], [30]) + @test all(isapprox.(getinfo(mpc_wd)[:Ŷ], 95-30; atol=1e-1)) + mpc_wr = LinMPC(model2, Nwt=[0], Cwt=Inf, Hp=50, Hc=50, Wr=[1], Wy=[1]) + mpc_wr = setconstraint!(mpc_wr, wmax=[175]) + preparestate!(mpc_wr, [50], [30]) + u = moveinput!(mpc_wr, [100], [30]) + @test all(isapprox.(getinfo(mpc_wr)[:Ŷ], 175-100; atol=1e-1)) end @testitem "LinMPC terminal cost" setup=[SetupMPCtests] begin From a0b027ae146fc9187ee064897ffd3fbd4359dc0a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 10:18:29 -0500 Subject: [PATCH 34/43] test: silence false positive linter in VS code --- test/3_test_predictive_control.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index ac4465b58..cd9dab185 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -1058,8 +1058,8 @@ end @testitem "NonLinMPC and ManualEstimator v.s. default" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(tf(5, [200, 1]), 300.0) - f(x,u,_,p) = p.A*x + p.Bu*u - h(x,_,p) = p.C*x + f = (x,u,_,p) -> p.A*x + p.Bu*u + h = (x,_,p) -> p.C*x model = setop!(NonLinModel(f, h, 300.0, 1, 1, 1; solver=nothing, p=linmodel), yop=[10]) r = [15] outdist = [5] From 60b13fbb8222f17222f88a7bd66f913f949cadc4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 10:35:22 -0500 Subject: [PATCH 35/43] test: seperating MHE construction --- test/2_test_state_estim.jl | 65 +++++++++++++++++++++----------------- 1 file changed, 36 insertions(+), 29 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 0c55da08c..98e3f4e9f 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -815,14 +815,10 @@ end @test_throws ErrorException setmodel!(ekf2, deepcopy(nonlinmodel)) end -@testitem "MovingHorizonEstimator construction" setup=[SetupMPCtests] begin +@testitem "MovingHorizonEstimator construction (LinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra - using JuMP, Ipopt, DifferentiationInterface import FiniteDiff linmodel = LinModel(sys,Ts,i_d=[3]) - f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d - h(x,d,model) = model.C*x + model.Dd*d - nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel) mhe1 = MovingHorizonEstimator(linmodel, He=5) @test mhe1.nym == 2 @@ -831,33 +827,26 @@ end @test mhe1.nx̂ == 6 @test size(mhe1.Ẽ, 2) == 6*mhe1.nx̂ - mhe2 = MovingHorizonEstimator(nonlinmodel, He=5) - @test mhe2.nym == 2 - @test mhe2.nyu == 0 - @test mhe2.nxs == 2 - @test mhe2.nx̂ == 6 - @test size(mhe1.Ẽ, 2) == 6*mhe1.nx̂ - - mhe3 = MovingHorizonEstimator(nonlinmodel, He=5, i_ym=[2]) + mhe3 = MovingHorizonEstimator(linmodel, He=5, i_ym=[2]) @test mhe3.nym == 1 @test mhe3.nyu == 1 @test mhe3.nxs == 1 @test mhe3.nx̂ == 5 - mhe4 = MovingHorizonEstimator(nonlinmodel, He=5, σQ=[1,2,3,4], σQint_ym=[5, 6], σR=[7, 8]) + mhe4 = MovingHorizonEstimator(linmodel, He=5, σQ=[1,2,3,4], σQint_ym=[5, 6], σR=[7, 8]) @test mhe4.cov.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) @test mhe4.cov.R̂ ≈ Hermitian(diagm(Float64[49, 64])) - mhe5 = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=[2,2]) + mhe5 = MovingHorizonEstimator(linmodel, He=5, nint_ym=[2,2]) @test mhe5.nxs == 4 @test mhe5.nx̂ == 8 - mhe6 = MovingHorizonEstimator(nonlinmodel, He=5, σP_0=[1,2,3,4], σPint_ym_0=[5,6]) + mhe6 = MovingHorizonEstimator(linmodel, He=5, σP_0=[1,2,3,4], σPint_ym_0=[5,6]) @test mhe6.cov.P̂_0 ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) @test mhe6.P̂arr_old ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) @test mhe6.cov.P̂_0 !== mhe6.P̂arr_old - mhe7 = MovingHorizonEstimator(nonlinmodel, He=10) + mhe7 = MovingHorizonEstimator(linmodel, He=10) @test mhe7.He == 10 @test length(mhe7.X̂0) == mhe7.He*6 @test length(mhe7.Y0m) == mhe7.He*2 @@ -865,12 +854,41 @@ end @test length(mhe7.D0) == (mhe7.He+mhe7.direct)*1 @test length(mhe7.Ŵ) == mhe7.He*6 - mhe8 = MovingHorizonEstimator(nonlinmodel, He=5, nint_u=[1, 1], nint_ym=[0, 0]) + mhe8 = MovingHorizonEstimator(linmodel, He=5, nint_u=[1, 1], nint_ym=[0, 0]) @test mhe8.nxs == 2 @test mhe8.nx̂ == 6 @test mhe8.nint_u == [1, 1] @test mhe8.nint_ym == [0, 0] + mhe12 = MovingHorizonEstimator(linmodel, He=5, Cwt=1e3) + @test size(mhe12.Ẽ, 2) == 6*mhe12.nx̂ + 1 + @test mhe12.C == 1e3 + + linmodel2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) + mhe13 = MovingHorizonEstimator(linmodel2, He=5) + @test isa(mhe13, MovingHorizonEstimator{Float32}) + + @test_throws ArgumentError MovingHorizonEstimator(linmodel) + @test_throws ArgumentError MovingHorizonEstimator(linmodel, He=0) + @test_throws ArgumentError MovingHorizonEstimator(linmodel, Cwt=-1) +end + +@testitem "MovingHorizonEstimator construction (NonLinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt, DifferentiationInterface + import FiniteDiff + linmodel = LinModel(sys,Ts,i_d=[3]) + f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d + h(x,d,model) = model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel) + + mhe2 = MovingHorizonEstimator(nonlinmodel, He=5) + @test mhe2.nym == 2 + @test mhe2.nyu == 0 + @test mhe2.nxs == 2 + @test mhe2.nx̂ == 6 + @test size(mhe2.Ẽ, 2) == 6*mhe2.nx̂ + I_6 = Matrix{Float64}(I, 6, 6) I_2 = Matrix{Float64}(I, 2, 2) optim = Model(Ipopt.Optimizer) @@ -886,14 +904,6 @@ end ) @test solver_name(mhe10.optim) == "Ipopt" - mhe12 = MovingHorizonEstimator(nonlinmodel, He=5, Cwt=1e3) - @test size(mhe12.Ẽ, 2) == 6*mhe12.nx̂ + 1 - @test mhe12.C == 1e3 - - linmodel2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) - mhe13 = MovingHorizonEstimator(linmodel2, He=5) - @test isa(mhe13, MovingHorizonEstimator{Float32}) - mhe14 = MovingHorizonEstimator( nonlinmodel, He=5, gradient=AutoFiniteDiff(), @@ -904,9 +914,6 @@ end @test mhe14.jacobian == AutoFiniteDiff() @test mhe14.hessian == AutoFiniteDiff() - @test_throws ArgumentError MovingHorizonEstimator(linmodel) - @test_throws ArgumentError MovingHorizonEstimator(linmodel, He=0) - @test_throws ArgumentError MovingHorizonEstimator(linmodel, Cwt=-1) @test_throws ErrorException MovingHorizonEstimator( nonlinmodel, 5, 1:2, 0, [1, 1], I_6, I_6, I_2, Inf; optim, covestim = InternalModel(nonlinmodel) From d446beb5f1b4a056bd004fe8155aed65feb191eb Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 10:41:12 -0500 Subject: [PATCH 36/43] test: seperating MHE estimate and `getinfo` --- test/2_test_state_estim.jl | 139 ++++++++++++++++++++----------------- 1 file changed, 74 insertions(+), 65 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 98e3f4e9f..2921b608f 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -924,9 +924,79 @@ end ) end -@testitem "MovingHorizonEstimator estimation and getinfo" setup=[SetupMPCtests] begin +@testitem "MovingHorizonEstimator estimation and getinfo (LinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff - using JuMP, Ipopt, DAQP + using JuMP, DAQP + linmodel = LinModel(sys,Ts,i_u=[1,2], i_d=[3]) + linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) + + mhe2 = MovingHorizonEstimator(linmodel, He=2) + preparestate!(mhe2, [50, 30], [5]) + x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5]) + @test x̂ ≈ zeros(6) atol=1e-9 + @test mhe2.x̂0 ≈ zeros(6) atol=1e-9 + preparestate!(mhe2, [50, 30], [5]) + info = getinfo(mhe2) + @test info[:x̂] ≈ x̂ atol=1e-9 + @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 + for i in 1:40 + preparestate!(mhe2, [50, 30], [5]) + updatestate!(mhe2, [11, 52], [50, 30], [5]) + end + preparestate!(mhe2, [50, 30], [5]) + @test mhe2([5]) ≈ [50, 30] atol=1e-3 + for i in 1:40 + preparestate!(mhe2, [51, 32], [5]) + updatestate!(mhe2, [10, 50], [51, 32], [5]) + end + preparestate!(mhe2, [51, 32], [5]) + @test mhe2([5]) ≈ [51, 32] atol=1e-3 + + mhe2 = MovingHorizonEstimator(linmodel, He=2, nint_u=[1, 1], nint_ym=[0, 0], direct=false) + preparestate!(mhe2, [50, 30], [5]) + x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5]) + @test x̂ ≈ zeros(6) atol=1e-9 + @test mhe2.x̂0 ≈ zeros(6) atol=1e-9 + info = getinfo(mhe2) + @test info[:x̂] ≈ x̂ atol=1e-9 + @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 + for i in 1:40 + preparestate!(mhe2, [50, 30], [5]) + updatestate!(mhe2, [11, 52], [50, 30], [5]) + end + @test mhe2([5]) ≈ [50, 30] atol=1e-2 + for i in 1:40 + preparestate!(mhe2, [51, 32], [5]) + updatestate!(mhe2, [10, 50], [51, 32], [5]) + end + @test mhe2([5]) ≈ [51, 32] atol=1e-2 + + Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2) + R̂ = diagm([1, 1].^2) + optim = Model(DAQP.Optimizer) + covestim = SteadyKalmanFilter(linmodel, 1:2, 0, 0, Q̂, R̂) + P̂_0 = covestim.cov.P̂ + mhe3 = MovingHorizonEstimator(linmodel, 2, 1:2, 0, 0, P̂_0, Q̂, R̂; optim, covestim) + preparestate!(mhe3, [50, 30], [5]) + x̂ = updatestate!(mhe3, [10, 50], [50, 30], [5]) + @test x̂ ≈ zeros(4) atol=1e-9 + @test mhe3.x̂0 ≈ zeros(4) atol=1e-9 + preparestate!(mhe3, [50, 30], [5]) + info = getinfo(mhe3) + @test info[:x̂] ≈ x̂ atol=1e-9 + @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 + + linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) + mhe3 = MovingHorizonEstimator(linmodel3, He=1) + preparestate!(mhe3, [0]) + x̂ = updatestate!(mhe3, [0], [0]) + @test x̂ ≈ [0, 0] atol=1e-3 + @test isa(x̂, Vector{Float32}) +end + +@testitem "MovingHorizonEstimator estimation and getinfo (NonLinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff + using JuMP, Ipopt linmodel = LinModel(sys,Ts,i_u=[1,2], i_d=[3]) linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d @@ -934,6 +1004,7 @@ end nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel) nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5]) + mhe1 = MovingHorizonEstimator(nonlinmodel, He=2) JuMP.set_attribute(mhe1.optim, "tol", 1e-7) preparestate!(mhe1, [50, 30], [5]) @@ -998,69 +1069,6 @@ end end @test mhe1c([5]) ≈ [51, 32] atol=1e-3 - mhe2 = MovingHorizonEstimator(linmodel, He=2) - preparestate!(mhe2, [50, 30], [5]) - x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5]) - @test x̂ ≈ zeros(6) atol=1e-9 - @test mhe2.x̂0 ≈ zeros(6) atol=1e-9 - preparestate!(mhe2, [50, 30], [5]) - info = getinfo(mhe2) - @test info[:x̂] ≈ x̂ atol=1e-9 - @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 - for i in 1:40 - preparestate!(mhe2, [50, 30], [5]) - updatestate!(mhe2, [11, 52], [50, 30], [5]) - end - preparestate!(mhe2, [50, 30], [5]) - @test mhe2([5]) ≈ [50, 30] atol=1e-3 - for i in 1:40 - preparestate!(mhe2, [51, 32], [5]) - updatestate!(mhe2, [10, 50], [51, 32], [5]) - end - preparestate!(mhe2, [51, 32], [5]) - @test mhe2([5]) ≈ [51, 32] atol=1e-3 - - mhe2 = MovingHorizonEstimator(linmodel, He=2, nint_u=[1, 1], nint_ym=[0, 0], direct=false) - preparestate!(mhe2, [50, 30], [5]) - x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5]) - @test x̂ ≈ zeros(6) atol=1e-9 - @test mhe2.x̂0 ≈ zeros(6) atol=1e-9 - info = getinfo(mhe2) - @test info[:x̂] ≈ x̂ atol=1e-9 - @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 - for i in 1:40 - preparestate!(mhe2, [50, 30], [5]) - updatestate!(mhe2, [11, 52], [50, 30], [5]) - end - @test mhe2([5]) ≈ [50, 30] atol=1e-2 - for i in 1:40 - preparestate!(mhe2, [51, 32], [5]) - updatestate!(mhe2, [10, 50], [51, 32], [5]) - end - @test mhe2([5]) ≈ [51, 32] atol=1e-2 - - Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2) - R̂ = diagm([1, 1].^2) - optim = Model(DAQP.Optimizer) - covestim = SteadyKalmanFilter(linmodel, 1:2, 0, 0, Q̂, R̂) - P̂_0 = covestim.cov.P̂ - mhe3 = MovingHorizonEstimator(linmodel, 2, 1:2, 0, 0, P̂_0, Q̂, R̂; optim, covestim) - preparestate!(mhe3, [50, 30], [5]) - x̂ = updatestate!(mhe3, [10, 50], [50, 30], [5]) - @test x̂ ≈ zeros(4) atol=1e-9 - @test mhe3.x̂0 ≈ zeros(4) atol=1e-9 - preparestate!(mhe3, [50, 30], [5]) - info = getinfo(mhe3) - @test info[:x̂] ≈ x̂ atol=1e-9 - @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 - - linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) - mhe3 = MovingHorizonEstimator(linmodel3, He=1) - preparestate!(mhe3, [0]) - x̂ = updatestate!(mhe3, [0], [0]) - @test x̂ ≈ [0, 0] atol=1e-3 - @test isa(x̂, Vector{Float32}) - Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2) R̂ = diagm([1, 1].^2) optim = Model(Ipopt.Optimizer) @@ -1092,6 +1100,7 @@ end @test x̂ ≈ zeros(6) atol=1e-9 @test_nowarn ModelPredictiveControl.info2debugstr(info) @test_throws ErrorException setstate!(mhe1, [1,2,3,4,5,6], diagm(.1:.1:.6)) + end @testitem "MovingHorizonEstimator estimation with unfilled window" setup=[SetupMPCtests] begin From cfadb0e253bbd27e32f6ea95b725f8b01343e5c3 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 11:03:05 -0500 Subject: [PATCH 37/43] test: seperating NMPC construction --- test/3_test_predictive_control.jl | 93 +++++++++++++++++-------------- 1 file changed, 51 insertions(+), 42 deletions(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index cd9dab185..7d3f868f2 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -757,62 +757,82 @@ end @test mpc.weights.L_Hp ≈ diagm(1.1:1000.1) end -@testitem "NonLinMPC construction" setup=[SetupMPCtests] begin +@testitem "NonLinMPC construction (LinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra - using JuMP, Ipopt, DifferentiationInterface - import FiniteDiff linmodel1 = LinModel(sys,Ts,i_d=[3]) nmpc0 = NonLinMPC(linmodel1, Hp=15) @test isa(nmpc0.estim, SteadyKalmanFilter) - f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d - h = (x,d,model) -> model.C*x + model.Dd*d - nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel1, solver=nothing) - nmpc1 = NonLinMPC(nonlinmodel, Hp=15) - @test isa(nmpc1.estim, UnscentedKalmanFilter) - @test size(nmpc1.R̂y, 1) == 15*nmpc1.estim.model.ny - nmpc2 = NonLinMPC(nonlinmodel, Hp=15, Hc=4, Cwt=Inf) - @test size(nmpc2.Ẽ, 2) == 4*nonlinmodel.nu - nmpc3 = NonLinMPC(nonlinmodel, Hp=15, Hc=4, Cwt=1e4) - @test size(nmpc3.Ẽ, 2) == 4*nonlinmodel.nu + 1 + nmpc3 = NonLinMPC(linmodel1, Hp=15, Hc=4, Cwt=1e4) + @test size(nmpc3.Ẽ, 2) == 4*linmodel1.nu + 1 @test nmpc3.weights.Ñ_Hc[end] == 1e4 - nmpc4 = NonLinMPC(nonlinmodel, Hp=15, Mwt=[1,2]) + nmpc4 = NonLinMPC(linmodel1, Hp=15, Mwt=[1,2]) @test nmpc4.weights.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15))) @test nmpc4.weights.M_Hp isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}} - nmpc5 = NonLinMPC(nonlinmodel, Hp=15 ,Nwt=[3,4], Cwt=1e3, Hc=5) + nmpc5 = NonLinMPC(linmodel1, Hp=15 ,Nwt=[3,4], Cwt=1e3, Hc=5) @test nmpc5.weights.Ñ_Hc ≈ Diagonal(diagm([repeat(Float64[3, 4], 5); [1e3]])) @test nmpc5.weights.Ñ_Hc isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}} - nmpc6 = NonLinMPC(nonlinmodel, Hp=15, Lwt=[0,1]) + nmpc6 = NonLinMPC(linmodel1, Hp=15, Lwt=[0,1]) @test nmpc6.weights.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15))) @test nmpc6.weights.L_Hp isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}} - nmpc7 = NonLinMPC(nonlinmodel, Hp=15, Ewt=1e-3, JE=(Ue,Ŷe,D̂e,p) -> p*dot(Ue,Ŷe)+sum(D̂e), p=10) + nmpc7 = NonLinMPC(linmodel1, Hp=15, Ewt=1e-3, JE=(Ue,Ŷe,D̂e,p) -> p*dot(Ue,Ŷe)+sum(D̂e), p=10) @test nmpc7.weights.E == 1e-3 @test nmpc7.JE([1,2],[3,4],[4,6],10) == 10*dot([1,2],[3,4])+sum([4,6]) - optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "nlp_scaling_max_gradient"=>1.0)) - nmpc8 = NonLinMPC(nonlinmodel, Hp=15, optim=optim) - @test solver_name(nmpc8.optim) == "Ipopt" - @test get_attribute(nmpc8.optim, "nlp_scaling_max_gradient") == 1.0 - im = InternalModel(nonlinmodel) - nmpc9 = NonLinMPC(im, Hp=15) - @test isa(nmpc9.estim, InternalModel) nmpc10 = NonLinMPC(linmodel1, nint_u=[1, 1], nint_ym=[0, 0]) @test nmpc10.estim.nint_u == [1, 1] @test nmpc10.estim.nint_ym == [0, 0] - nmpc11 = NonLinMPC(nonlinmodel, Hp=15, nint_u=[1, 1], nint_ym=[0, 0]) - @test nmpc11.estim.nint_u == [1, 1] - @test nmpc11.estim.nint_ym == [0, 0] - nmpc12 = NonLinMPC(nonlinmodel, Hp=10, M_Hp=Hermitian(diagm(1.01:0.01:1.2), :L)) + nmpc12 = NonLinMPC(linmodel1, Hp=10, M_Hp=Hermitian(diagm(1.01:0.01:1.2), :L)) @test nmpc12.weights.M_Hp ≈ diagm(1.01:0.01:1.2) @test nmpc12.weights.M_Hp isa Hermitian{Float64, Matrix{Float64}} - nmpc13 = NonLinMPC(nonlinmodel, Hp=10, N_Hc=Hermitian(diagm([0.1,0.11,0.12,0.13]), :L), Cwt=Inf) + nmpc13 = NonLinMPC(linmodel1, Hp=10, N_Hc=Hermitian(diagm([0.1,0.11,0.12,0.13]), :L), Cwt=Inf) @test nmpc13.weights.Ñ_Hc ≈ diagm([0.1,0.11,0.12,0.13]) @test nmpc13.weights.Ñ_Hc isa Hermitian{Float64, Matrix{Float64}} - nmcp14 = NonLinMPC(nonlinmodel, Hp=10, L_Hp=Hermitian(diagm(0.001:0.001:0.02), :L)) + nmcp14 = NonLinMPC(linmodel1, Hp=10, L_Hp=Hermitian(diagm(0.001:0.001:0.02), :L)) @test nmcp14.weights.L_Hp ≈ diagm(0.001:0.001:0.02) @test nmcp14.weights.L_Hp isa Hermitian{Float64, Matrix{Float64}} - nmpc15 = NonLinMPC(nonlinmodel, Hp=10, gc=(Ue,Ŷe,D̂e,p,ϵ)-> [p*dot(Ue,Ŷe)+sum(D̂e)+ϵ], nc=1, p=10) + nmpc15 = NonLinMPC(linmodel1, Hp=10, gc=(Ue,Ŷe,D̂e,p,ϵ)-> [p*dot(Ue,Ŷe)+sum(D̂e)+ϵ], nc=1, p=10) LHS = zeros(1) nmpc15.con.gc!(LHS,[1,2],[3,4],[4,6],10,0.1) @test LHS ≈ [10*dot([1,2],[3,4])+sum([4,6])+0.1] + nmpc17 = NonLinMPC(linmodel1, Hp=10, transcription=MultipleShooting()) + @test nmpc17.transcription == MultipleShooting() + @test length(nmpc17.Z̃) == linmodel1.nu*nmpc17.Hc + nmpc17.estim.nx̂*nmpc17.Hp + nmpc17.nϵ + @test size(nmpc17.con.Aeq, 1) == nmpc17.estim.nx̂*nmpc17.Hp + + @test_throws DimensionMismatch NonLinMPC(linmodel1, Hp=15, Ewt=[1, 1]) + @test_throws ErrorException NonLinMPC(linmodel1, Hp=15, JE = (_,_,_)->0.0) + @test_throws ErrorException NonLinMPC(linmodel1, Hp=15, gc = (_,_,_,_)->[0.0], nc=1) + @test_throws ErrorException NonLinMPC(linmodel1, Hp=15, gc! = (_,_,_,_)->[0.0], nc=1) + + @test_logs (:warn, Regex(".*")) NonLinMPC(linmodel1, Hp=15, JE=(Ue,_,_,_)->Ue) + @test_logs (:warn, Regex(".*")) NonLinMPC(linmodel1, Hp=15, gc=(Ue,_,_,_,_)->Ue, nc=0) +end + +@testitem "NonLinMPC construction (NonLinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt, DifferentiationInterface + import FiniteDiff + linmodel1 = LinModel(sys,Ts,i_d=[3]) + nmpc0 = NonLinMPC(linmodel1, Hp=15) + @test isa(nmpc0.estim, SteadyKalmanFilter) + f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d + h = (x,d,model) -> model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel1, solver=nothing) + + nmpc1 = NonLinMPC(nonlinmodel, Hp=15) + @test isa(nmpc1.estim, UnscentedKalmanFilter) + @test size(nmpc1.R̂y, 1) == 15*nmpc1.estim.model.ny + nmpc2 = NonLinMPC(nonlinmodel, Hp=15, Hc=4, Cwt=Inf) + @test size(nmpc2.Ẽ, 2) == 4*nonlinmodel.nu + optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "nlp_scaling_max_gradient"=>1.0)) + nmpc8 = NonLinMPC(nonlinmodel, Hp=15, optim=optim) + @test solver_name(nmpc8.optim) == "Ipopt" + @test get_attribute(nmpc8.optim, "nlp_scaling_max_gradient") == 1.0 + im = InternalModel(nonlinmodel) + nmpc9 = NonLinMPC(im, Hp=15) + @test isa(nmpc9.estim, InternalModel) + nmpc11 = NonLinMPC(nonlinmodel, Hp=15, nint_u=[1, 1], nint_ym=[0, 0]) + @test nmpc11.estim.nint_u == [1, 1] + @test nmpc11.estim.nint_ym == [0, 0] gc! = (LHS,_,_,_,_,_)-> (LHS .= 0.0) # useless, only for coverage nmpc16 = NonLinMPC(nonlinmodel, Hp=10, transcription=MultipleShooting(), nc=10, gc=gc!) @test nmpc16.transcription == MultipleShooting() @@ -825,10 +845,6 @@ end @test length(nmpc16.Z̃) == nonlinmodel_c.nu*nmpc16.Hc + nmpc16.estim.nx̂*nmpc16.Hp + nmpc16.nϵ @test nmpc16.con.neq == nmpc16.estim.nx̂*nmpc16.Hp @test nmpc16.con.nc == 10 - nmpc17 = NonLinMPC(linmodel1, Hp=10, transcription=MultipleShooting()) - @test nmpc17.transcription == MultipleShooting() - @test length(nmpc17.Z̃) == linmodel1.nu*nmpc17.Hc + nmpc17.estim.nx̂*nmpc17.Hp + nmpc17.nϵ - @test size(nmpc17.con.Aeq, 1) == nmpc17.estim.nx̂*nmpc17.Hp nmpc18 = NonLinMPC(nonlinmodel, Hp=10, gradient=AutoFiniteDiff(), jacobian=AutoFiniteDiff(), @@ -850,17 +866,10 @@ end @test isa(nmpc15.estim, UnscentedKalmanFilter{Float32}) @test isa(nmpc15.optim, JuMP.GenericModel{Float64}) # Ipopt does not support Float32 - @test_throws DimensionMismatch NonLinMPC(nonlinmodel, Hp=15, Ewt=[1, 1]) @test_throws ArgumentError NonLinMPC(nonlinmodel) - @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, JE = (_,_,_)->0.0) - @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, gc = (_,_,_,_)->[0.0], nc=1) - @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, gc! = (_,_,_,_)->[0.0], nc=1) @test_throws ArgumentError NonLinMPC(nonlinmodel, transcription=TrapezoidalCollocation()) @test_throws ArgumentError NonLinMPC(nonlinmodel, transcription=TrapezoidalCollocation(2)) @test_throws ErrorException NonLinMPC(linmodel1, oracle=false, hessian=AutoFiniteDiff()) - - @test_logs (:warn, Regex(".*")) NonLinMPC(nonlinmodel, Hp=15, JE=(Ue,_,_,_)->Ue) - @test_logs (:warn, Regex(".*")) NonLinMPC(nonlinmodel, Hp=15, gc=(Ue,_,_,_,_)->Ue, nc=0) end @testitem "NonLinMPC moves and getinfo" setup=[SetupMPCtests] begin From 2b73756e2267e1c050fdc7e0364558bdf98eac3f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 11:15:19 -0500 Subject: [PATCH 38/43] test: seperating NMPC moves and `getinfo` --- test/3_test_predictive_control.jl | 73 ++++++++++++++++++++----------- 1 file changed, 47 insertions(+), 26 deletions(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 7d3f868f2..b9b5a89d0 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -872,11 +872,12 @@ end @test_throws ErrorException NonLinMPC(linmodel1, oracle=false, hessian=AutoFiniteDiff()) end -@testitem "NonLinMPC moves and getinfo" setup=[SetupMPCtests] begin +@testitem "NonLinMPC moves and getinfo (LinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra using DifferentiationInterface import FiniteDiff linmodel = setop!(LinModel(tf(5, [2000, 1]), 3000.0), yop=[10]) + Hp = 100 nmpc_lin = NonLinMPC(linmodel, Nwt=[0], Hp=Hp, Hc=1) ry, ru = [15], [4] @@ -898,6 +899,7 @@ end end R̂y, R̂u = fill(ry[1], Hp), fill(ru[1], Hp) p = [1, R̂y, 0, R̂u] + nmpc = NonLinMPC(linmodel, Mwt=[0], Nwt=[0], Cwt=Inf, Ewt=1, JE=JE, p=p, Hp=Hp, Hc=1) preparestate!(nmpc, [10]) u = moveinput!(nmpc) @@ -907,10 +909,43 @@ end nmpc.p .= [0, R̂y, 1, R̂u] u = moveinput!(nmpc) @test u ≈ [4] atol=5e-2 + + linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), 0, 0, 3000.0) + nmpc6 = NonLinMPC(linmodel3, Hp=10) + preparestate!(nmpc6, [0]) + @test moveinput!(nmpc6, [0]) ≈ [0.0] atol=5e-2 + + nmpc9 = NonLinMPC(linmodel, Nwt=[0], Hp=100, Hc=1, transcription=MultipleShooting()) + preparestate!(nmpc9, [10]) + u = moveinput!(nmpc9, [20]) + @test u ≈ [2] atol=5e-2 + info = getinfo(nmpc9) + @test info[:u] ≈ u + @test info[:Ŷ][end] ≈ 20 atol=5e-2 + + # coverage of the branch with error termination status (with an infeasible problem): + nmpc_infeas = NonLinMPC(linmodel, Hp=1, Hc=1, Cwt=Inf) + nmpc_infeas = setconstraint!(nmpc_infeas, umin=[+1], umax=[-1]) + preparestate!(nmpc_infeas, [0], [0]) + @test_logs( + (:error, "MPC terminated without solution: returning last solution shifted "* + "(more info in debug log)"), + moveinput!(nmpc_infeas, [0], [0]) + ) + + @test_nowarn ModelPredictiveControl.info2debugstr(info) +end + + +@testitem "NonLinMPC moves and getinfo (NonLinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using DifferentiationInterface + import FiniteDiff linmodel2 = LinModel([tf(5, [2000, 1]) tf(7, [8000,1])], 3000.0, i_d=[2]) f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d h = (x,d,model) -> model.C*x + model.Dd*d nonlinmodel = NonLinModel(f, h, 3000.0, 1, 2, 1, 1, solver=nothing, p=linmodel2) + nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=100, Hc=1) preparestate!(nmpc2, [0], [0]) # if d=[0.1], the output will eventually reach 7*0.1=0.7, no action needed (u=0): @@ -922,40 +957,43 @@ end info = getinfo(nmpc2) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ 7d[1] atol=5e-2 + nmpc3 = NonLinMPC(nonlinmodel, Nwt=[0], Cwt=Inf, Hp=100, Hc=1) preparestate!(nmpc3, [0], [0]) u = moveinput!(nmpc3, 7d, d) @test u ≈ [0] atol=5e-2 + nmpc4 = NonLinMPC(nonlinmodel, Hp=15, Mwt=[0], Nwt=[0], Lwt=[1]) preparestate!(nmpc4, [0], [0]) u = moveinput!(nmpc4, [0], d, R̂u=fill(12, nmpc4.Hp)) @test u ≈ [12] atol=5e-2 + nmpc5 = NonLinMPC(nonlinmodel, Hp=1, Hc=1, Cwt=Inf, transcription=MultipleShooting()) nmpc5 = setconstraint!(nmpc5, ymin=[1]) f! = (ẋ,x,u,_,_) -> ẋ .= -0.001x .+ u h! = (y,x,_,_) -> y .= x nonlinmodel_c = NonLinModel(f!, h!, 500, 1, 1, 1) transcription = TrapezoidalCollocation(0, f_threads=true, h_threads=true) + nmpc5 = NonLinMPC(nonlinmodel_c; Nwt=[0], Hp=100, Hc=1, transcription) preparestate!(nmpc5, [0.0]) u = moveinput!(nmpc5, [1/0.001]) @test u ≈ [1.0] atol=5e-2 - transcription = TrapezoidalCollocation(1) + + transcription = TrapezoidalCollocation(1) nmpc5_1 = NonLinMPC(nonlinmodel_c; Nwt=[0], Hp=100, Hc=1, transcription) preparestate!(nmpc5_1, [0.0]) u = moveinput!(nmpc5_1, [1/0.001]) @test u ≈ [1.0] atol=5e-2 - linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), 0, 0, 3000.0) - nmpc6 = NonLinMPC(linmodel3, Hp=10) - preparestate!(nmpc6, [0]) - @test moveinput!(nmpc6, [0]) ≈ [0.0] atol=5e-2 nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing, p=linmodel2) + nmpc7 = NonLinMPC(nonlinmodel2, Hp=10) y = similar(nonlinmodel2.yop) ModelPredictiveControl.h!(y, nonlinmodel2, Float32[0,0], Float32[0], nonlinmodel2.p) preparestate!(nmpc7, [0], [0]) @test moveinput!(nmpc7, [0], [0]) ≈ [0.0] atol=5e-2 transcription = MultipleShooting() + nmpc8 = NonLinMPC(nonlinmodel; Nwt=[0], Hp=100, Hc=1, transcription) preparestate!(nmpc8, [0], [0]) u = moveinput!(nmpc8, [10], [0]) @@ -963,6 +1001,7 @@ end info = getinfo(nmpc8) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ 10 atol=5e-2 + transcription = MultipleShooting(f_threads=true, h_threads=true) nmpc8t = NonLinMPC(nonlinmodel; Nwt=[0], Hp=100, Hc=1, transcription, hessian=true) nmpc8t = setconstraint!(nmpc8t, ymax=[100], ymin=[-100]) # coverage of getinfo! Hessians of Lagrangian @@ -972,13 +1011,7 @@ end info = getinfo(nmpc8t) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ 10 atol=5e-2 - nmpc9 = NonLinMPC(linmodel, Nwt=[0], Hp=100, Hc=1, transcription=MultipleShooting()) - preparestate!(nmpc9, [10]) - u = moveinput!(nmpc9, [20]) - @test u ≈ [2] atol=5e-2 - info = getinfo(nmpc9) - @test info[:u] ≈ u - @test info[:Ŷ][end] ≈ 20 atol=5e-2 + nmpc10 = setconstraint!(NonLinMPC( nonlinmodel, Nwt=[0], Hp=100, Hc=1, gradient=AutoFiniteDiff(), @@ -992,24 +1025,12 @@ end info = getinfo(nmpc10) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ 10 atol=5e-2 + nmpc11 = NonLinMPC(nonlinmodel, Hp=10, Hc=[1, 2, 3, 4], Nwt=[10]) preparestate!(nmpc11, y, [0]) moveinput!(nmpc11, [10], [0]) ΔU_diff = diff(getinfo(nmpc11)[:U]) @test ΔU_diff[[2, 4, 5, 7, 8, 9]] ≈ zeros(6) atol=1e-9 - - # coverage of the branch with error termination status (with an infeasible problem): - nmpc_infeas = NonLinMPC(nonlinmodel, Hp=1, Hc=1, Cwt=Inf) - nmpc_infeas = setconstraint!(nmpc_infeas, umin=[+1], umax=[-1]) - preparestate!(nmpc_infeas, [0], [0]) - @test_logs( - (:error, "MPC terminated without solution: returning last solution shifted "* - "(more info in debug log)"), - moveinput!(nmpc_infeas, [0], [0]) - ) - - - @test_nowarn ModelPredictiveControl.info2debugstr(info) end @testitem "NonLinMPC step disturbance rejection" setup=[SetupMPCtests] begin From 4a95ae3c4571d3d30620a656679022fafd95cfac Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 11:31:44 -0500 Subject: [PATCH 39/43] test: seperating NMPC constrain violation --- test/3_test_predictive_control.jl | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index b9b5a89d0..0aa7fafb3 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -926,17 +926,16 @@ end # coverage of the branch with error termination status (with an infeasible problem): nmpc_infeas = NonLinMPC(linmodel, Hp=1, Hc=1, Cwt=Inf) nmpc_infeas = setconstraint!(nmpc_infeas, umin=[+1], umax=[-1]) - preparestate!(nmpc_infeas, [0], [0]) + preparestate!(nmpc_infeas, [0]) @test_logs( (:error, "MPC terminated without solution: returning last solution shifted "* "(more info in debug log)"), - moveinput!(nmpc_infeas, [0], [0]) + moveinput!(nmpc_infeas, [0]) ) @test_nowarn ModelPredictiveControl.info2debugstr(info) end - @testitem "NonLinMPC moves and getinfo (NonLinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra using DifferentiationInterface @@ -1263,7 +1262,7 @@ end end -@testitem "NonLinMPC constraint violation" setup=[SetupMPCtests] begin +@testitem "NonLinMPC constraint violation (LinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP gc(Ue, Ŷe, _ ,p , ϵ) = [p[1]*(Ue[1:end-1] .- 4.2 .- ϵ); p[2]*(Ŷe[2:end] .- 3.14 .- ϵ)] Hp=50 @@ -1336,7 +1335,14 @@ end info = getinfo(nmpc_lin) @test all(isapprox.(info[:Ŷ], 3.14; atol=1e-1)) @test all(isapprox.(info[:gc][Hp+1:end], 0.0; atol=1e-1)) +end +@testitem "NonLinMPC constraint violation (NonLinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP + gc(Ue, Ŷe, _ ,p , ϵ) = [p[1]*(Ue[1:end-1] .- 4.2 .- ϵ); p[2]*(Ŷe[2:end] .- 3.14 .- ϵ)] + Hp=50 + + linmodel = LinModel(tf([2], [10000, 1]), 3000.0) f = (x,u,_,p) -> p.A*x + p.Bu*u h = (x,_,p) -> p.C*x nonlinmodel = NonLinModel(f, h, linmodel.Ts, 1, 1, 1, solver=nothing, p=linmodel) From 407411d461e31da541ce3ca479d9018e79fe26bd Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 13:27:15 -0500 Subject: [PATCH 40/43] test: `NonLinMPC` custom linear constraints violation --- src/controller/execute.jl | 39 ++++++++++++++++++++++++++++ src/controller/transcription.jl | 42 ++++--------------------------- test/3_test_predictive_control.jl | 22 ++++++++++++++++ 3 files changed, 66 insertions(+), 37 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index b1e6b6e31..8342f6924 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -303,6 +303,45 @@ end "Fill `Ŷs` vector with 0 values when `estim` is not an [`InternalModel`](@ref)." predictstoch!(Ŷs, mpc::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing) +@doc raw""" + linconstraint_custom!(mpc::PredictiveController, model::SimModel) + +Init the ``\mathbf{F_w}`` vector for the custom linear inequality constraints. + +See [`relaxW`](@ref) for the definition of the vector. The function does nothing if +`mpc.con.nw < 1`. +""" +function linconstraint_custom!(mpc::PredictiveController, model::SimModel) + mpc.con.nw < 1 && return nothing + ny, nu, nd, buffer = model.ny, model.nu, model.nd, mpc.buffer + Fw = mpc.con.Fw + Ue_term, D̂e_term, R̂e_term = buffer.Ue, buffer.D̂e, buffer.Ŷe + Fw .= 0 + Ue_term[1:end-nu] .= mpc.Tu_lastu0 .+ mpc.Uop + Ue_term[end-nu+1:end] .= mpc.lastu0 .+ model.uop + mul!(Fw, mpc.con.W̄u, Ue_term, 1, 1) + if model.nd > 0 + D̂e_term[1:nd] .= mpc.d0 .+ model.dop + D̂e_term[nd+1:end] .= mpc.D̂0 .+ mpc.Dop + mul!(Fw, mpc.con.W̄d, D̂e_term, 1, 1) + end + R̂e_term[1:ny] .= mpc.ry + R̂e_term[ny+1:end] .= mpc.R̂y + mul!(Fw, mpc.con.W̄r, R̂e_term, 1, 1) + return linconstraint_custom_outputs!(mpc, model) +end + +"Also include the `W̄y` term in the custom linear constraints for [`LinModel`](@ref)." +function linconstraint_custom_outputs!(mpc::PredictiveController, model::LinModel) + Ŷe_term, Fw, ny = mpc.buffer.Ŷe, mpc.con.Fw, model.ny + Ŷe_term[1:ny] .= mpc.ŷ + Ŷe_term[ny+1:end] .= mpc.F .+ mpc.Yop + mul!(Fw, mpc.con.W̄y, Ŷe_term, 1, 1) + return nothing +end +"Do nothing for other model types." +linconstraint_custom_outputs!(::PredictiveController, ::SimModel) = nothing + """ extended_vectors!(Ue, Ŷe, mpc::PredictiveController, U0, Ŷ0) -> Ue, Ŷe diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index a27644a13..d18d4365d 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -787,7 +787,7 @@ function linconstraint!(mpc::PredictiveController, model::LinModel, ::Transcript mul!(fx̂, mpc.con.gx̂, mpc.d0, 1, 1) mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1) end - mpc.con.nw > 0 && linconstraint_custom!(mpc, model) + linconstraint_custom!(mpc, model) n = 0 mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0 n += nU @@ -816,12 +816,12 @@ function linconstraint!(mpc::PredictiveController, model::LinModel, ::Transcript end "Set `b` excluding predicted output constraints for `NonLinModel` and not `SingleShooting`." -function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::TranscriptionMethod) +function linconstraint!(mpc::PredictiveController, model::NonLinModel, ::TranscriptionMethod) nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin) nW = length(mpc.con.Wmin) nx̂ = mpc.estim.nx̂ # here, updating fx̂ is not necessary since fx̂ = 0 - mpc.con.nw > 0 && linconstraint_custom!(mpc, model) + linconstraint_custom!(mpc, model) n = 0 mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0 n += nU @@ -845,10 +845,10 @@ function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::Transcriptio end "Also exclude terminal constraints for `NonLinModel` and `SingleShooting`." -function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::SingleShooting) +function linconstraint!(mpc::PredictiveController, model::NonLinModel, ::SingleShooting) nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin) nW = length(mpc.con.Wmin) - mpc.con.nw > 0 && linconstraint_custom!(mpc, model) + linconstraint_custom!(mpc, model) n = 0 mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0 n += nU @@ -868,38 +868,6 @@ function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::SingleShooti return nothing end -"Init the ``\\mathbf{F_w}`` vector for the linear model custom inequality constraints." -function linconstraint_custom!(mpc::PredictiveController, model::SimModel) - ny, nu, nd, buffer = model.ny, model.nu, model.nd, mpc.buffer - Fw = mpc.con.Fw - Ue_term, D̂e_term, R̂e_term = buffer.Ue, buffer.D̂e, buffer.Ŷe - Fw .= 0 - Ue_term[1:end-nu] .= mpc.Tu_lastu0 .+ mpc.Uop - Ue_term[end-nu+1:end] .= mpc.lastu0 .+ model.uop - mul!(Fw, mpc.con.W̄u, Ue_term, 1, 1) - if model.nd > 0 - D̂e_term[1:nd] .= mpc.d0 .+ model.dop - D̂e_term[nd+1:end] .= mpc.D̂0 .+ mpc.Dop - mul!(Fw, mpc.con.W̄d, D̂e_term, 1, 1) - end - R̂e_term[1:ny] .= mpc.ry - R̂e_term[ny+1:end] .= mpc.R̂y - mul!(Fw, mpc.con.W̄r, R̂e_term, 1, 1) - return linconstraint_custom_outputs!(mpc, model) -end - -"Also include the `W̄y` term in the custom linear constraints for [`LinModel`](@ref)." -function linconstraint_custom_outputs!(mpc::PredictiveController, model::LinModel) - Ŷe_term, Fw, ny = mpc.buffer.Ŷe, mpc.con.Fw, model.ny - Ŷe_term[1:ny] .= mpc.ŷ - Ŷe_term[ny+1:end] .= mpc.F .+ mpc.Yop - mul!(Fw, mpc.con.W̄y, Ŷe_term, 1, 1) - return nothing -end -"Do nothing for other model types." -linconstraint_custom_outputs!(::PredictiveController, ::SimModel) = nothing - - @doc raw""" linconstrainteq!( mpc::PredictiveController, model::LinModel, transcription::MultipleShooting diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 0aa7fafb3..392be5fcd 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -1523,6 +1523,28 @@ end @test all(isapprox.(info[:Ŷ], 3.14; atol=1e-1)) @test all(isapprox.(info[:gc][Hp+1:end], 0.0; atol=1e-1)) + linmodel2 = LinModel([tf([2], [2500, 1]) tf(0.1, [2000, 1])], 3000.0, i_d=[2]) + f = (x,u,d,p) -> p.A*x + p.Bu*u + p.Bd*d + h = (x,d,p) -> p.C*x + p.Dd*d + nonlinmodel2 = NonLinModel(f, h, linmodel2.Ts, 1, 2, 1, 1, solver=nothing, p=linmodel2) + nonlinmodel2 = setop!(nonlinmodel2, uop=[25], dop=[30], yop=[50]) + nmpc_wu = NonLinMPC(nonlinmodel2, Nwt=[0], Cwt=Inf, Hp=100, Hc=1, Wu=[1]) + nmpc_wu = setconstraint!(nmpc_wu, wmax=[20]) + preparestate!(nmpc_wu, [50], [30]) + u = moveinput!(nmpc_wu, [100], [30]) + @test all(isapprox.(getinfo(nmpc_wu)[:U], 20.0; atol=1e-1)) + nmpc_wd = NonLinMPC(nonlinmodel2, Nwt=[0], Cwt=Inf, Hp=100, Hc=1, Wu=[1], Wd=[1]) + nmpc_wd = setconstraint!(nmpc_wd, wmax=[45]) + preparestate!(nmpc_wd, [50], [30]) + u = moveinput!(nmpc_wd, [100], [30]) + @test all(isapprox.(getinfo(nmpc_wd)[:U], 45-30; atol=1e-1)) + nmpc_wr = NonLinMPC(nonlinmodel2, Nwt=[0], Cwt=Inf, Hp=100, Hc=1, Wu=[1], Wr=[1]) + nmpc_wr = setconstraint!(nmpc_wr, wmax=[145]) + preparestate!(nmpc_wr, [50], [30]) + u = moveinput!(nmpc_wr, [100], [30]) + @show getinfo(nmpc_wr)[:U] + @test all(isapprox.(getinfo(nmpc_wr)[:U], 145-100; atol=1e-1)) + end @testitem "NonLinMPC set model" setup=[SetupMPCtests] begin From b403fa43f5502724f6005c82656eb55ed79ca476 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 13:28:08 -0500 Subject: [PATCH 41/43] test: remove useless print --- test/3_test_predictive_control.jl | 1 - 1 file changed, 1 deletion(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 392be5fcd..e48d24e13 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -1542,7 +1542,6 @@ end nmpc_wr = setconstraint!(nmpc_wr, wmax=[145]) preparestate!(nmpc_wr, [50], [30]) u = moveinput!(nmpc_wr, [100], [30]) - @show getinfo(nmpc_wr)[:U] @test all(isapprox.(getinfo(nmpc_wr)[:U], 145-100; atol=1e-1)) end From ee864358fbdc9ad5e5a23ed0205dc733a118653a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 14:19:42 -0500 Subject: [PATCH 42/43] test: `LinMPC` custom linear constraints with lower bound --- test/3_test_predictive_control.jl | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index e48d24e13..80c8f7860 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -452,23 +452,31 @@ end model2 = LinModel([tf([2], [10, 1]) tf(0.1, [7, 1])], 3.0, i_d=[2]) model2 = setop!(model2, uop=[25], dop=[30], yop=[50]) mpc_wy = LinMPC(model2, Nwt=[0], Cwt=Inf, Hp=50, Hc=50, Wy=[1]) - mpc_wy = setconstraint!(mpc_wy, wmax=[75]) + mpc_wy = setconstraint!(mpc_wy, wmin=[36], wmax=[75]) preparestate!(mpc_wy, [50], [30]) + u = moveinput!(mpc_wy, [0], [30]) + @test all(isapprox.(getinfo(mpc_wy)[:Ŷ], 36; atol=1e-1)) u = moveinput!(mpc_wy, [100], [30]) - @test all(isapprox.(getinfo(mpc_wy)[:Ŷ], 75.0; atol=1e-1)) + @test all(isapprox.(getinfo(mpc_wy)[:Ŷ], 75; atol=1e-1)) mpc_wu = LinMPC(model2, Nwt=[0], Cwt=Inf, Hp=50, Hc=50, Wu=[1]) - mpc_wu = setconstraint!(mpc_wu, wmax=[20]) + mpc_wu = setconstraint!(mpc_wu, wmin=[4], wmax=[20]) preparestate!(mpc_wu, [50], [30]) + u = moveinput!(mpc_wu, [0], [30]) + @test all(isapprox.(getinfo(mpc_wu)[:U], 4; atol=1e-1)) u = moveinput!(mpc_wu, [100], [30]) - @test all(isapprox.(getinfo(mpc_wu)[:U], 20.0; atol=1e-1)) + @test all(isapprox.(getinfo(mpc_wu)[:U], 20; atol=1e-1)) mpc_wd = LinMPC(model2, Nwt=[0], Cwt=Inf, Hp=50, Hc=50, Wd=[1], Wy=[1]) - mpc_wd = setconstraint!(mpc_wd, wmax=[95]) + mpc_wd = setconstraint!(mpc_wd, wmin=[56], wmax=[95]) preparestate!(mpc_wd, [50], [30]) + u = moveinput!(mpc_wd, [0], [30]) + @test all(isapprox.(getinfo(mpc_wd)[:Ŷ], 56-30; atol=1e-1)) u = moveinput!(mpc_wd, [100], [30]) @test all(isapprox.(getinfo(mpc_wd)[:Ŷ], 95-30; atol=1e-1)) mpc_wr = LinMPC(model2, Nwt=[0], Cwt=Inf, Hp=50, Hc=50, Wr=[1], Wy=[1]) - mpc_wr = setconstraint!(mpc_wr, wmax=[175]) + mpc_wr = setconstraint!(mpc_wr, wmin=[52], wmax=[175]) preparestate!(mpc_wr, [50], [30]) + u = moveinput!(mpc_wr, [21], [30]) + @test all(isapprox.(getinfo(mpc_wr)[:Ŷ], 52-21; atol=1e-1)) u = moveinput!(mpc_wr, [100], [30]) @test all(isapprox.(getinfo(mpc_wr)[:Ŷ], 175-100; atol=1e-1)) end From b4ba4998f169737c05233d5cea22b83c1509dc51 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 16:17:10 -0500 Subject: [PATCH 43/43] test: improve coverage --- test/3_test_predictive_control.jl | 1 + 1 file changed, 1 insertion(+) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 80c8f7860..e8f413966 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -878,6 +878,7 @@ end @test_throws ArgumentError NonLinMPC(nonlinmodel, transcription=TrapezoidalCollocation()) @test_throws ArgumentError NonLinMPC(nonlinmodel, transcription=TrapezoidalCollocation(2)) @test_throws ErrorException NonLinMPC(linmodel1, oracle=false, hessian=AutoFiniteDiff()) + @test_throws ArgumentError NonLinMPC(nonlinmodel, Wy=[1 0;0 1]) end @testitem "NonLinMPC moves and getinfo (LinModel)" setup=[SetupMPCtests] begin