test: NonLinMPC custom linear constraints violation

Author: franckgaga

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
 

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

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