Merge pull request #310 from JuliaControl/custom_linconstraint

added: custom linear inequality constraints for `LinMPC` and `NonLinMPC`

Author: franckgaga

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`.

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:
 

docs/src/internals/predictive_control.md

@@ -20,7 +20,9 @@ ModelPredictiveControl.init_defectmat
 ModelPredictiveControl.relaxU
 ModelPredictiveControl.relaxΔU
 ModelPredictiveControl.relaxŶ
+ModelPredictiveControl.relaxW
 ModelPredictiveControl.relaxterminal
+ModelPredictiveControl.augmentdefect
 ModelPredictiveControl.init_quadprog
 ModelPredictiveControl.init_stochpred
 ModelPredictiveControl.init_matconstraint_mpc
@@ -31,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!
 ```

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]

src/controller/construct.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)
+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
 
 """
-    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)
@@ -300,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
 
@@ -626,18 +668,28 @@ 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
     mpc.J .= J
     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ϵ)
@@ -647,15 +699,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 .= [

src/controller/execute.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,
@@ -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. 

src/controller/explicitmpc.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
@@ -48,7 +49,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,
+        Wy, Wu, Wd, Wr,
         transcription::TM, optim::JM
     ) where {
             NT<:Real, 
@@ -58,26 +60,27 @@ struct LinMPC{
             JM<:JuMP.GenericModel
         }
         model = estim.model
-        nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂
-        ŷ = copy(model.yop) # dummy vals (updated just before optimization)
+        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)
+        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)
         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ŝ,
+            Wy, Wu, Wd, Wr
         )
         H̃ = init_quadprog(model, transcription, weights, Ẽ, P̃Δu, P̃u)
         # dummy vals (updated just before optimization):
@@ -91,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,
@@ -147,6 +150,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 
@@ -157,6 +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}``.
+- `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
@@ -191,6 +199,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 `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.
+
     The objective function follows this nomenclature:
 
     | VARIABLE             | DESCRIPTION                                              | SIZE             |
@@ -221,13 +234,20 @@ 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)),
+    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),
     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, Wy, Wu, Wd, Wr, transcription, optim
+    )
 end
 
 
@@ -270,9 +290,13 @@ 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)),
+    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),
+    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)
@@ -283,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, transcription, optim)
+    return LinMPC{NT}(estim, Hp, Hc, nb, weights, Wy, Wu, Wd, Wr, transcription, optim)
 end
 
 """