Merge pull request #176 from JuliaControl/debug_ms_terminal_nonlinmodel

franckgaga · web-flow · commit b84d375b0fae · 2025-03-16T16:37:52.000-04:00
debug: terminal constraint now works with `MultipleShooting` + `NonLinModel`
diff --git a/Project.toml b/Project.toml
@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "1.4.2"
+version = "1.4.3"
 
 [deps]
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md
@@ -30,7 +30,7 @@ ModelPredictiveControl.init_nonlincon!
 
 ```@docs
 ModelPredictiveControl.initpred!(::PredictiveController, ::LinModel, ::Any, ::Any, ::Any, ::Any)
-ModelPredictiveControl.linconstraint!(::PredictiveController, ::LinModel)
+ModelPredictiveControl.linconstraint!(::PredictiveController, ::LinModel, ::TranscriptionMethod)
 ModelPredictiveControl.linconstrainteq!
 ```
 
diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl
@@ -32,7 +32,7 @@ export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFi
 export MovingHorizonEstimator
 export default_nint, initstate!
 export PredictiveController, ExplicitMPC, LinMPC, NonLinMPC, setconstraint!, moveinput!
-export SingleShooting, MultipleShooting
+export TranscriptionMethod, SingleShooting, MultipleShooting
 export SimResult, getinfo, sim!
 
 include("general.jl")
diff --git a/src/controller/construct.jl b/src/controller/construct.jl
@@ -386,7 +386,7 @@ function setconstraint!(
         JuMP.delete(optim, optim[:linconstraint])
         JuMP.unregister(optim, :linconstraint)
         @constraint(optim, linconstraint, A*Z̃var .≤ b)
-        set_nonlincon!(mpc, model, optim)
+        set_nonlincon!(mpc, model, transcription, optim)
     else
         i_b, i_g = init_matconstraint_mpc(
             model, transcription, nc,
@@ -400,94 +400,6 @@ function setconstraint!(
     return mpc
 end
 
-
-@doc raw"""
-    init_matconstraint_mpc(
-        model::LinModel, transcription::TranscriptionMethod, nc::Int,
-        i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, 
-        args...
-    ) -> i_b, i_g, A, Aeq, neq
-
-Init `i_b`, `i_g`, `neq`, and `A` and `Aeq` matrices for the all the MPC constraints.
-
-The linear and nonlinear constraints are respectively defined as:
-```math
-\begin{aligned} 
-    \mathbf{A Z̃ }       &≤ \mathbf{b}           \\ 
-    \mathbf{A_{eq} Z̃}   &= \mathbf{b_{eq}}      \\
-    \mathbf{g(Z̃)}       &≤ \mathbf{0}           \\
-    \mathbf{g_{eq}(Z̃)}  &= \mathbf{0}           \\
-\end{aligned}
-```
-The argument `nc` is the number of custom nonlinear inequality constraints in
-``\mathbf{g_c}``. `i_b` is a `BitVector` including the indices of ``\mathbf{b}`` that are
-finite numbers. `i_g` is a similar vector but for the indices of ``\mathbf{g}``. The method
-also returns the ``\mathbf{A, A_{eq}}`` matrices 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_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ`. 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_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, 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_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ = args
-        A   = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ymin; A_Ymax; A_x̂min; A_x̂max]
-        Aeq = A_ŝ
-        neq = 0
-    end
-    i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Ymin; i_Ymax; i_x̂min; i_x̂max]
-    i_g = trues(nc)
-    return i_b, i_g, A, Aeq, neq
-end
-
-"Init `i_b` without output constraints if [`NonLinModel`](@ref) & [`MultipleShooting`](@ref)."
-function init_matconstraint_mpc(
-    ::NonLinModel{NT}, ::MultipleShooting, nc::Int,
-    i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, 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_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ = args
-        A   = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ymin; A_Ymax; A_x̂min; A_x̂max]
-        Aeq = A_ŝ
-        nΔŨ, nZ̃ = size(A_ΔŨmin)
-        neq = nZ̃ - nΔŨ
-    end
-    i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_x̂min; i_x̂max]
-    i_g = [i_Ymin; i_Ymax; trues(nc)]
-    return i_b, i_g, A, Aeq, neq
-end
-
-"Init `i_b` without output & terminal constraints if [`NonLinModel`](@ref) & [`SingleShooting`](@ref)."
-function init_matconstraint_mpc(
-    ::NonLinModel{NT}, ::SingleShooting, nc::Int,
-    i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, 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_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ = args
-        A   = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ymin; A_Ymax; A_x̂min; A_x̂max]
-        Aeq = A_ŝ
-        neq = 0
-    end
-    i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax]
-    i_g = [i_Ymin; i_Ymax; i_x̂min;  i_x̂max; trues(nc)]
-    return i_b, i_g, A, Aeq, neq
-end
-
-
-"By default, there is no nonlinear constraint, thus do nothing."
-set_nonlincon!(::PredictiveController, ::SimModel, ::JuMP.GenericModel) = nothing
-
 """
     default_Hp(model::LinModel)
 
diff --git a/src/controller/execute.jl b/src/controller/execute.jl
@@ -66,7 +66,7 @@ function moveinput!(
     end
     validate_args(mpc, ry, d, D̂, R̂y, R̂u)
     initpred!(mpc, mpc.estim.model, d, D̂, R̂y, R̂u)
-    linconstraint!(mpc, mpc.estim.model)
+    linconstraint!(mpc, mpc.estim.model, mpc.transcription)
     linconstrainteq!(mpc, mpc.estim.model, mpc.transcription)
     Z̃ = optim_objective!(mpc)
     return getinput(mpc, Z̃)
@@ -269,66 +269,6 @@ end
 "Fill `Ŷs` vector with 0 values when `estim` is not an [`InternalModel`](@ref)."
 predictstoch!(Ŷs, mpc::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing)
 
-@doc raw"""
-    linconstraint!(mpc::PredictiveController, model::LinModel)
-
-Set `b` vector for the linear model inequality constraints (``\mathbf{A Z̃ ≤ b}``).
-
-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).
-"""
-function linconstraint!(mpc::PredictiveController, model::LinModel)
-    nU, nΔŨ, nY = length(mpc.con.U0min), length(mpc.con.ΔŨmin), length(mpc.con.Y0min)
-    nx̂, fx̂ = mpc.estim.nx̂, mpc.con.fx̂
-    fx̂ .= mpc.con.bx̂
-    mul!(fx̂, mpc.con.kx̂, mpc.estim.x̂0, 1, 1)
-    mul!(fx̂, mpc.con.vx̂, mpc.estim.lastu0, 1, 1)
-    if model.nd ≠ 0
-        mul!(fx̂, mpc.con.gx̂, mpc.d0, 1, 1)
-        mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1)
-    end
-    n = 0
-    mpc.con.b[(n+1):(n+nU)]  .= @. -mpc.con.U0min + mpc.Tu_lastu0
-    n += nU
-    mpc.con.b[(n+1):(n+nU)]  .= @. +mpc.con.U0max - mpc.Tu_lastu0
-    n += nU
-    mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin
-    n += nΔŨ
-    mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax
-    n += nΔŨ
-    mpc.con.b[(n+1):(n+nY)]  .= @. -mpc.con.Y0min + mpc.F
-    n += nY
-    mpc.con.b[(n+1):(n+nY)]  .= @. +mpc.con.Y0max - mpc.F
-    n += nY
-    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̂
-    if any(mpc.con.i_b) 
-        lincon = mpc.optim[:linconstraint]
-        JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b])
-    end
-    return nothing
-end
-
-"Set `b` excluding predicted output constraints when `model` is not a [`LinModel`](@ref)."
-function linconstraint!(mpc::PredictiveController, ::SimModel)
-    nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin)
-    n = 0
-    mpc.con.b[(n+1):(n+nU)]  .= @. -mpc.con.U0min + mpc.Tu_lastu0
-    n += nU
-    mpc.con.b[(n+1):(n+nU)]  .= @. +mpc.con.U0max - mpc.Tu_lastu0
-    n += nU
-    mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin
-    n += nΔŨ
-    mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax
-    if any(mpc.con.i_b) 
-        lincon = mpc.optim[:linconstraint]
-        @views JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b])
-    end
-    return nothing
-end
-
 """
     extended_vectors!(Ue, Ŷe, mpc::PredictiveController, U0, Ŷ0) -> Ue, Ŷe
 
diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl
@@ -182,7 +182,7 @@ function Base.show(io::IO, mpc::ExplicitMPC)
     print_estim_dim(io, mpc.estim, n)
 end
 
-linconstraint!(::ExplicitMPC, ::LinModel) = nothing
+linconstraint!(::ExplicitMPC, ::LinModel, ::TranscriptionMethod) = nothing
 
 @doc raw"""
     optim_objective!(mpc::ExplicitMPC) -> Z̃
diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl
@@ -509,7 +509,7 @@ function init_optimization!(mpc::NonLinMPC, model::SimModel, optim)
     @operator(optim, J, nZ̃, Jfunc, ∇Jfunc!)
     @objective(optim, Min, J(Z̃var...))
     init_nonlincon!(mpc, model, transcription, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs!)
-    set_nonlincon!(mpc, model, optim)
+    set_nonlincon!(mpc, model, transcription, optim)
     return nothing
 end
 
@@ -692,60 +692,6 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
     return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs!
 end
 
-"""
-    set_nonlincon!(mpc::NonLinMPC, ::LinModel, optim)
-
-Set the custom nonlinear inequality constraints for `LinModel`.
-"""
-function set_nonlincon!(
-    mpc::NonLinMPC, ::LinModel, optim::JuMP.GenericModel{JNT}
-) where JNT<:Real
-    Z̃var = optim[:Z̃var]
-    con = mpc.con
-    nonlin_constraints = JuMP.all_constraints(optim, JuMP.NonlinearExpr, MOI.LessThan{JNT})
-    map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints)
-    for i in 1:con.nc
-        gfunc_i = optim[Symbol("g_c_$i")]
-        @constraint(optim, gfunc_i(Z̃var...) <= 0)
-    end
-    return nothing
-end
-
-"""
-    set_nonlincon!(mpc::NonLinMPC, ::NonLinModel, optim)
-
-Also set output prediction `Ŷ` and terminal state `x̂end` constraints when not a `LinModel`.
-"""
-function set_nonlincon!(
-    mpc::NonLinMPC, ::SimModel, optim::JuMP.GenericModel{JNT}
-) where JNT<:Real
-    Z̃var = optim[:Z̃var]
-    con = mpc.con
-    nonlin_constraints = JuMP.all_constraints(optim, JuMP.NonlinearExpr, MOI.LessThan{JNT})
-    map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints)
-    for i in findall(.!isinf.(con.Y0min))
-        gfunc_i = optim[Symbol("g_Y0min_$(i)")]
-        @constraint(optim, gfunc_i(Z̃var...) <= 0)
-    end
-    for i in findall(.!isinf.(con.Y0max))
-        gfunc_i = optim[Symbol("g_Y0max_$(i)")]
-        @constraint(optim, gfunc_i(Z̃var...) <= 0)
-    end
-    for i in findall(.!isinf.(con.x̂0min))
-        gfunc_i = optim[Symbol("g_x̂0min_$(i)")]
-        @constraint(optim, gfunc_i(Z̃var...) <= 0)
-    end
-    for i in findall(.!isinf.(con.x̂0max))
-        gfunc_i = optim[Symbol("g_x̂0max_$(i)")]
-        @constraint(optim, gfunc_i(Z̃var...) <= 0)
-    end
-    for i in 1:con.nc
-        gfunc_i = optim[Symbol("g_c_$i")]
-        @constraint(optim, gfunc_i(Z̃var...) <= 0)
-    end
-    return nothing
-end
-
 """
     con_nonlinprog!(g, mpc::NonLinMPC, model::LinModel, _ , _ , gc, ϵ) -> g
 
diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl
diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl
