From 3f4af695b24a4d58e0f30c965264efc5c9861751 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 16 Mar 2025 12:00:34 -0400 Subject: [PATCH 1/9] debug: terminal constraint now works with `MultipleShooting` + `NonLinModel` --- src/controller/construct.jl | 6 +-- src/controller/nonlinmpc.jl | 56 +------------------ src/controller/transcription.jl | 89 +++++++++++++++++++++++++++++++ test/3_test_predictive_control.jl | 49 ++++++++++++++--- 4 files changed, 134 insertions(+), 66 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 77f3431b..1ecba425 100644 --- 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, optim, transcription) else i_b, i_g = init_matconstraint_mpc( model, transcription, nc, @@ -484,10 +484,6 @@ function init_matconstraint_mpc( 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/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 269925d8..04e195e2 100644 --- 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, optim, transcription) 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 `Ŷ` 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 index e84aae76..9ee44570 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -650,6 +650,95 @@ function init_nonlincon!( return nothing end + +"By default, there is no nonlinear constraint, thus do nothing." +function set_nonlincon!( + ::PredictiveController, ::SimModel, ::JuMP.GenericModel, ::TranscriptionMethod + ) + return nothing +end + +""" + set_nonlincon!(mpc::NonLinMPC, ::LinModel, ::TranscriptionMethod, optim) + +Set the custom nonlinear inequality constraints for `LinModel`. +""" +function set_nonlincon!( + mpc::NonLinMPC, ::LinModel, ::TranscriptionMethod, 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, ::MultipleShooting, optim) + +Also set output prediction `Ŷ` constraints for `NonLinModel` and `MultipleShooting`. +""" +function set_nonlincon!( + mpc::NonLinMPC, ::SimModel, ::MultipleShooting, 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 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, ::SingleShooting, optim) + +Also set output prediction `Ŷ` and terminal state `x̂end` constraint for `SingleShooting`. +""" +function set_nonlincon!( + mpc::NonLinMPC, ::NonLinModel, ::SingleShooting, 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 + @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 1c265b57..8cec1be1 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -794,7 +794,17 @@ end setconstraint!(nmpc_lin, ymin=[5,10],ymax=[55, 35]) @test all((nmpc_lin.con.Y0min, nmpc_lin.con.Y0max) .≈ ([5,10], [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 all((-nmpc_lin.con.A_Ymin[:, end], -nmpc_lin.con.A_Ymax[:, end]) .≈ + ([1.0,1.1], [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])) + 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])) f = (x,u,d,_) -> linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d h = (x,d,_) -> linmodel1.C*x + linmodel1.Dd*d @@ -808,17 +818,44 @@ end setconstraint!(nmpc, ymin=[-6, -11],ymax=[55, 35]) @test all((nmpc.con.Y0min, nmpc.con.Y0max) .≈ ([-6,-11], [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 all((nmpc.con.x̂0min, nmpc.con.x̂0max) .≈ + ([-21,-22,-23,-24,-25,-26], [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 all((-nmpc.con.A_Umin[:, end], -nmpc.con.A_Umax[:, end]) .≈ + ([0.01,0.02], [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 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])) 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])) - 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])) + 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])) + + 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])) + 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])) + + 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])) + 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])) end From c246ddb95e1b6e54f29d72129b852b84b7de7dc3 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 16 Mar 2025 12:22:23 -0400 Subject: [PATCH 2/9] debug: correction with `include` order --- src/controller/transcription.jl | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 9ee44570..2eaec419 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -659,13 +659,14 @@ function set_nonlincon!( end """ - set_nonlincon!(mpc::NonLinMPC, ::LinModel, ::TranscriptionMethod, optim) + set_nonlincon!(mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, optim) Set the custom nonlinear inequality constraints for `LinModel`. """ function set_nonlincon!( - mpc::NonLinMPC, ::LinModel, ::TranscriptionMethod, optim::JuMP.GenericModel{JNT} + mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, ::JuMP.GenericModel{JNT} ) where JNT<:Real + optim = mpc.optim Z̃var = optim[:Z̃var] con = mpc.con nonlin_constraints = JuMP.all_constraints(optim, JuMP.NonlinearExpr, MOI.LessThan{JNT}) @@ -678,13 +679,14 @@ function set_nonlincon!( end """ - set_nonlincon!(mpc::NonLinMPC, ::NonLinModel, ::MultipleShooting, optim) + set_nonlincon!(mpc::PredictiveController, ::NonLinModel, ::MultipleShooting, optim) Also set output prediction `Ŷ` constraints for `NonLinModel` and `MultipleShooting`. """ function set_nonlincon!( - mpc::NonLinMPC, ::SimModel, ::MultipleShooting, optim::JuMP.GenericModel{JNT} + mpc::PredictiveController, ::SimModel, ::MultipleShooting, ::JuMP.GenericModel{JNT} ) where JNT<:Real + optim = mpc.optim Z̃var = optim[:Z̃var] con = mpc.con nonlin_constraints = JuMP.all_constraints(optim, JuMP.NonlinearExpr, MOI.LessThan{JNT}) @@ -705,13 +707,14 @@ function set_nonlincon!( end """ - set_nonlincon!(mpc::NonLinMPC, ::NonLinModel, ::SingleShooting, optim) + set_nonlincon!(mpc::PredictiveController, ::NonLinModel, ::SingleShooting, optim) Also set output prediction `Ŷ` and terminal state `x̂end` constraint for `SingleShooting`. """ function set_nonlincon!( - mpc::NonLinMPC, ::NonLinModel, ::SingleShooting, optim::JuMP.GenericModel{JNT} + mpc::PredictiveController, ::NonLinModel, ::SingleShooting, ::JuMP.GenericModel{JNT} ) where JNT<:Real + optim = mpc.optim Z̃var = optim[:Z̃var] con = mpc.con nonlin_constraints = JuMP.all_constraints(optim, JuMP.NonlinearExpr, MOI.LessThan{JNT}) From dcf5839ed0ed9618964b9c9fe0cd29b1321c38d7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 16 Mar 2025 12:49:29 -0400 Subject: [PATCH 3/9] debug: add dispatch on `transcription` for `linconstraint` --- src/controller/execute.jl | 62 +--------------------- src/controller/explicitmpc.jl | 2 +- src/controller/transcription.jl | 92 +++++++++++++++++++++++++++++++++ 3 files changed, 94 insertions(+), 62 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 7becca12..d9176d3e 100644 --- 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 `Ŷs` vector with 0 values when `estim` is not an [`InternalModel`](@ref)." predictstoch!(Ŷs, mpc::PredictiveController, ::StateEstimator) = (Ŷ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ΔŨ, nY = length(mpc.con.U0min), length(mpc.con.ΔŨ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ΔŨ)] .= @. -mpc.con.ΔŨmin - n += nΔŨ - mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax - n += nΔŨ - 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ΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨ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ΔŨ)] .= @. -mpc.con.ΔŨmin - n += nΔŨ - mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨ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, Ŷe, mpc::PredictiveController, U0, Ŷ0) -> Ue, Ŷe diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index ffbd9107..60ab4d20 100644 --- 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/transcription.jl b/src/controller/transcription.jl index 2eaec419..b4ad04f6 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -742,6 +742,98 @@ function set_nonlincon!( return nothing end +@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, ::TranscriptionMethod) + nU, nΔŨ, nY = length(mpc.con.U0min), length(mpc.con.ΔŨ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ΔŨ)] .= @. -mpc.con.ΔŨmin + n += nΔŨ + mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax + n += nΔŨ + 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 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̂ + 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ΔŨ)] .= @. -mpc.con.ΔŨmin + n += nΔŨ + mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax + n += nΔŨ + 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 +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 = 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ΔŨ)] .= @. -mpc.con.ΔŨmin + n += nΔŨ + mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨ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 + + + + @doc raw""" linconstrainteq!( mpc::PredictiveController, model::LinModel, transcription::MultipleShooting From 87a5eb1b22f3c1d3cdfe1f5827732505a8a85105 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 16 Mar 2025 13:24:54 -0400 Subject: [PATCH 4/9] debug: adequate terminal state matrices for `NonLinModel` and `MultipleShooting` --- Project.toml | 2 +- src/controller/construct.jl | 84 -------------------------- src/controller/transcription.jl | 101 ++++++++++++++++++++++++++++++-- 3 files changed, 98 insertions(+), 89 deletions(-) diff --git a/Project.toml b/Project.toml index 3aeb327c..0927b31f 100644 --- 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/src/controller/construct.jl b/src/controller/construct.jl index 1ecba425..fff40f6b 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -400,90 +400,6 @@ function setconstraint!( return mpc 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, - 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_ΔŨ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}}``. -""" -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, - 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] - 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_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_ΔŨmin, i_ΔŨ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_ΔŨ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] - 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_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_ΔŨmin, i_ΔŨ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_ΔŨ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] - Aeq = A_ŝ - neq = 0 - end - i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax] - i_g = [i_Ymin; i_Ymax; i_x̂min; i_x̂max; trues(nc)] - return i_b, i_g, A, Aeq, neq -end - """ default_Hp(model::LinModel) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index b4ad04f6..6cdf0149 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -375,9 +375,15 @@ end @doc raw""" init_predmat(model::SimModel, estim, transcription::MultipleShooting, Hp, Hc) -Return empty matrices except `ex̂` for [`SimModel`](@ref) and [`MultipleShooting`](@ref). +Return the terminal state matrices for [`SimModel`](@ref) and [`MultipleShooting`](@ref). -The matrix is ``\mathbf{e_x̂} = [\begin{smallmatrix}\mathbf{0} & \mathbf{I}\end{smallmatrix}]``. +The output prediction matrices are all empty matrices. The terminal state matrices are +given in the Extended Help section. + +# Extended Help +!!! details "Extended Help" + The terminal state matrices all appropriately sized zero matrices ``\mathbf{0}``, except + for ``\mathbf{e_x̂} = [\begin{smallmatrix}\mathbf{0} & \mathbf{I}\end{smallmatrix}]`` """ function init_predmat( model::SimModel, estim::StateEstimator{NT}, transcription::MultipleShooting, Hp, Hc @@ -391,7 +397,11 @@ function init_predmat( V = zeros(NT, 0, nu) B = zeros(NT, 0) ex̂ = [zeros(NT, nx̂, Hc*nu + (Hp-1)*nx̂) I] - gx̂, jx̂, kx̂, vx̂, bx̂ = G, J, K, V, B + gx̂ = zeros(NT, nx̂, nd) + jx̂ = zeros(NT, nx̂, nd*Hp) + kx̂ = zeros(NT, nx̂, nx̂) + vx̂ = zeros(NT, nx̂, nu) + bx̂ = zeros(NT, nx̂) return E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ end @@ -508,6 +518,89 @@ function init_defectmat( return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ 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, + 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_ΔŨ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}}``. +""" +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, + 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] + 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_g = trues(nc) + return i_b, i_g, A, Aeq, neq +end + +"Init `i_b` without output constraints if `NonLinModel` and not `SingleShooting`." +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, + 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] + 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_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` 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, + 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] + Aeq = A_ŝ + neq = 0 + end + i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax] + i_g = [i_Ymin; i_Ymax; i_x̂min; i_x̂max; trues(nc)] + return i_b, i_g, A, Aeq, neq +end + """ init_nonlincon!( mpc::PredictiveController, model::LinModel, transcription::TranscriptionMethod, @@ -785,7 +878,7 @@ 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ΔŨ, nY = length(mpc.con.U0min), length(mpc.con.ΔŨmin), length(mpc.con.Y0min) nx̂, fx̂ = mpc.estim.nx̂, mpc.con.fx̂ fx̂ .= mpc.con.bx̂ From 85c8f6f6a0fc5c025824b46a7c1c602e13a9874c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 16 Mar 2025 13:31:04 -0400 Subject: [PATCH 5/9] =?UTF-8?q?changed:=20=20updating=20`fx=CC=82`=20is=20?= =?UTF-8?q?not=20necessary=20for=20non-`SingleShooting`=20It=20is=20always?= =?UTF-8?q?=20equal=20to=20zero?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/transcription.jl | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 6cdf0149..f9dd2bb9 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -881,13 +881,7 @@ end function linconstraint!(mpc::PredictiveController, model::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̂ - 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 + # here, updating fx̂ is not necessary since fx̂ = 0 n = 0 mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0 n += nU From d0c71e3c2a8240f81ad97daaa4df225145cfd256 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 16 Mar 2025 13:31:49 -0400 Subject: [PATCH 6/9] changed: remove useless addition --- src/controller/transcription.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index f9dd2bb9..1fada806 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -891,9 +891,9 @@ function linconstraint!(mpc::PredictiveController, model::NonLinModel, ::Transcr n += nΔŨ mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax n += nΔŨ - mpc.con.b[(n+1):(n+nx̂)] .= @. -mpc.con.x̂0min + fx̂ + mpc.con.b[(n+1):(n+nx̂)] .= @. -mpc.con.x̂0min n += nx̂ - mpc.con.b[(n+1):(n+nx̂)] .= @. +mpc.con.x̂0max - fx̂ + mpc.con.b[(n+1):(n+nx̂)] .= @. +mpc.con.x̂0max if any(mpc.con.i_b) lincon = mpc.optim[:linconstraint] JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b]) From fcc133337716a9f04bfd10737968a11e3e9eb47c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 16 Mar 2025 14:21:01 -0400 Subject: [PATCH 7/9] debug: correct argument order for `set_nonlincon!` --- src/controller/construct.jl | 2 +- src/controller/nonlinmpc.jl | 2 +- src/controller/transcription.jl | 8 +++++--- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index fff40f6b..16b7b745 100644 --- 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, transcription) + set_nonlincon!(mpc, model, transcription, optim) else i_b, i_g = init_matconstraint_mpc( model, transcription, nc, diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 04e195e2..4713769c 100644 --- 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, transcription) + set_nonlincon!(mpc, model, transcription, optim) return nothing end diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 1fada806..ee20f71c 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -613,8 +613,10 @@ Init nonlinear constraints for [`LinModel`](@ref) for all [`TranscriptionMethod` The only nonlinear constraints are the custom inequality constraints `gc`. """ function init_nonlincon!( - mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, gfuncs, ∇gfuncs!, _ , _ - ) + mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, + gfuncs, ∇gfuncs!, + _ , _ +) optim, con = mpc.optim, mpc.con nZ̃ = length(mpc.Z̃) if length(con.i_g) ≠ 0 @@ -746,7 +748,7 @@ end "By default, there is no nonlinear constraint, thus do nothing." function set_nonlincon!( - ::PredictiveController, ::SimModel, ::JuMP.GenericModel, ::TranscriptionMethod + ::PredictiveController, ::SimModel, ::TranscriptionMethod, ::JuMP.GenericModel, ) return nothing end From 424b7c3439ffcd6e60ed48505689c8a8a3e070f0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 16 Mar 2025 14:44:43 -0400 Subject: [PATCH 8/9] test: improve coverage --- test/3_test_predictive_control.jl | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 8cec1be1..0d30cb9f 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -865,8 +865,8 @@ end Hp=50 linmodel = LinModel(tf([2], [10000, 1]), 3000.0) - nmpc_lin = NonLinMPC(linmodel, Hp=Hp, Hc=5, gc=gc, nc=2Hp, p=[0; 0]) - + nmpc_lin = NonLinMPC(linmodel; Hp, Hc=5, gc=gc, nc=2Hp, p=[0; 0]) + setconstraint!(nmpc_lin, x̂min=[-1e6,-Inf], x̂max=[1e6,+Inf]) setconstraint!(nmpc_lin, umin=[-10], umax=[10]) setconstraint!(nmpc_lin, Δumin=[-1e6], Δumax=[1e6]) @@ -935,7 +935,7 @@ end 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) - nmpc = NonLinMPC(nonlinmodel, Hp=50, Hc=5, gc=gc, nc=2Hp, p=[0; 0]) + nmpc = NonLinMPC(nonlinmodel; Hp, Hc=5, gc=gc, nc=2Hp, p=[0; 0]) setconstraint!(nmpc, x̂min=[-1e6,-Inf], x̂max=[+1e6,+Inf]) setconstraint!(nmpc, umin=[-1e6], umax=[+1e6]) @@ -1001,6 +1001,18 @@ end info = getinfo(nmpc) @test all(isapprox.(info[:Ŷ], 3.14; atol=1e-1)) @test all(isapprox.(info[:gc][Hp+1:end], 0.0; atol=1e-1)) + + nmpc_ms = NonLinMPC(nonlinmodel; Hp, Hc=5, transcription=MultipleShooting()) + + preparestate!(nmpc_ms, [0]) + + setconstraint!(nmpc_ms, x̂min=[-1e-6,-Inf], x̂max=[+1e-6,+Inf]) + moveinput!(nmpc_ms, [-10]) + info = getinfo(nmpc_ms) + @test info[:x̂end][1] ≈ 0 atol=1e-1 + moveinput!(nmpc_ms, [10]) + info = getinfo(nmpc_ms) + @test info[:x̂end][1] ≈ 0 atol=1e-1 end From 85f89ffe155664edd2ed7f1373a1e6c2359c5715 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 16 Mar 2025 15:25:30 -0400 Subject: [PATCH 9/9] debug: resolve ambiguous methods --- docs/src/internals/predictive_control.md | 2 +- src/ModelPredictiveControl.jl | 2 +- src/controller/transcription.jl | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index eb49a954..d15e9f95 100644 --- 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 index 95d8acf2..bf65b67c 100644 --- 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/transcription.jl b/src/controller/transcription.jl index ee20f71c..63839999 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -776,10 +776,10 @@ end """ set_nonlincon!(mpc::PredictiveController, ::NonLinModel, ::MultipleShooting, optim) -Also set output prediction `Ŷ` constraints for `NonLinModel` and `MultipleShooting`. +Also set output prediction `Ŷ` constraints for `NonLinModel` and non-`SingleShooting`. """ function set_nonlincon!( - mpc::PredictiveController, ::SimModel, ::MultipleShooting, ::JuMP.GenericModel{JNT} + mpc::PredictiveController, ::NonLinModel, ::TranscriptionMethod, ::JuMP.GenericModel{JNT} ) where JNT<:Real optim = mpc.optim Z̃var = optim[:Z̃var]