From c4ca823950789bbc527c5d0ca252857bb42dd053 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 16 Mar 2025 18:55:05 -0400 Subject: [PATCH 1/5] debug: ineq. constraint with `NonLinModel` and `MultipleShooting` I forgot that I need a new dispatch in `con_nonlinprog!`, since the terminal constraints are linear for `NonLinModel` thus they do not appear there. --- src/controller/nonlinmpc.jl | 49 +------------------ src/controller/transcription.jl | 84 +++++++++++++++++++++++++++++++++ 2 files changed, 85 insertions(+), 48 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 4713769c..36b86a0c 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -582,7 +582,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃) Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ) - g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ) + g = con_nonlinprog!(g, mpc, model, transcription, x̂0end, Ŷ0, gc, ϵ) geq = con_nonlinprogeq!(geq, X̂0, Û0, mpc, model, transcription, U0, Z̃) end return nothing @@ -692,53 +692,6 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! end -""" - con_nonlinprog!(g, mpc::NonLinMPC, model::LinModel, _ , _ , gc, ϵ) -> g - -Nonlinear constrains for [`NonLinMPC`](@ref) when `model` is a [`LinModel`](@ref). - -The method mutates the `g` vectors in argument and returns it. Only the custom constraints -are include in the `g` vector. -""" -function con_nonlinprog!(g, mpc::NonLinMPC, ::LinModel, _ , _ , gc, ϵ) - for i in eachindex(g) - g[i] = gc[i] - end - return g -end - -""" - con_nonlinprog!(g, mpc::NonLinMPC, model::SimModel, x̂0end, Ŷ0, gc, ϵ) -> g - -Nonlinear constrains for [`NonLinMPC`](@ref) when `model` is not a [`LinModel`](@ref). - -The method mutates the `g` vectors in argument and returns it. The output prediction, -the terminal state and the custom constraints are include in the `g` vector. -""" -function con_nonlinprog!(g, mpc::NonLinMPC, ::SimModel, x̂0end, Ŷ0, gc, ϵ) - nx̂, nŶ = length(x̂0end), length(Ŷ0) - for i in eachindex(g) - mpc.con.i_g[i] || continue - if i ≤ nŶ - j = i - g[i] = (mpc.con.Y0min[j] - Ŷ0[j]) - ϵ*mpc.con.C_ymin[j] - elseif i ≤ 2nŶ - j = i - nŶ - g[i] = (Ŷ0[j] - mpc.con.Y0max[j]) - ϵ*mpc.con.C_ymax[j] - elseif i ≤ 2nŶ + nx̂ - j = i - 2nŶ - g[i] = (mpc.con.x̂0min[j] - x̂0end[j]) - ϵ*mpc.con.c_x̂min[j] - elseif i ≤ 2nŶ + 2nx̂ - j = i - 2nŶ - nx̂ - g[i] = (x̂0end[j] - mpc.con.x̂0max[j]) - ϵ*mpc.con.c_x̂max[j] - else - j = i - 2nŶ - 2nx̂ - g[i] = gc[j] - end - end - return g -end - @doc raw""" con_custom!(gc, mpc::NonLinMPC, Ue, Ŷe, ϵ) -> gc diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 63839999..fdb7fb00 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1116,6 +1116,90 @@ function predict!( return Ŷ0, x̂0end end +""" + con_nonlinprog!( + g, mpc::PredictiveController, model::LinModel, ::TranscriptionMethod, _ , _ , gc, ϵ + ) -> g + +Nonlinear constrains when `model` is a [`LinModel`](@ref). + +The method mutates the `g` vectors in argument and returns it. Only the custom constraints +are include in the `g` vector. +""" +function con_nonlinprog!( + g, mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , _ , gc, ϵ +) + for i in eachindex(g) + g[i] = gc[i] + end + return g +end + +""" + con_nonlinprog!( + g, mpc::PredictiveController, model::NonLinModel, ::TranscriptionMethod, x̂0end, Ŷ0, gc, ϵ + ) -> g + +Nonlinear constrains when `model` is a [`NonLinModel`](@ref) with non-[`SingleShooting`](@ref). + +The method mutates the `g` vectors in argument and returns it. The output prediction and the +custom constraints are include in the `g` vector. +""" +function con_nonlinprog!( + g, mpc::PredictiveController, ::NonLinModel, ::TranscriptionMethod, x̂0end, Ŷ0, gc, ϵ +) + nx̂, nŶ = length(x̂0end), length(Ŷ0) + for i in eachindex(g) + mpc.con.i_g[i] || continue + if i ≤ nŶ + j = i + g[i] = (mpc.con.Y0min[j] - Ŷ0[j]) - ϵ*mpc.con.C_ymin[j] + elseif i ≤ 2nŶ + j = i - nŶ + g[i] = (Ŷ0[j] - mpc.con.Y0max[j]) - ϵ*mpc.con.C_ymax[j] + else + j = i - 2nŶ - 2nx̂ + g[i] = gc[j] + end + end + return g +end + +""" + con_nonlinprog!( + g, mpc::PredictiveController, model::NonLinModel, ::SingleShooting, x̂0end, Ŷ0, gc, ϵ + ) -> g + +Nonlinear constrains when `model` is [`NonLinModel`](@ref) with [`SingleShooting`](@ref). + +The method mutates the `g` vectors in argument and returns it. The output prediction, +the terminal state and the custom constraints are include in the `g` vector. +""" +function con_nonlinprog!( + g, mpc::PredictiveController, ::NonLinModel, ::SingleShooting, x̂0end, Ŷ0, gc, ϵ +) + nx̂, nŶ = length(x̂0end), length(Ŷ0) + for i in eachindex(g) + mpc.con.i_g[i] || continue + if i ≤ nŶ + j = i + g[i] = (mpc.con.Y0min[j] - Ŷ0[j]) - ϵ*mpc.con.C_ymin[j] + elseif i ≤ 2nŶ + j = i - nŶ + g[i] = (Ŷ0[j] - mpc.con.Y0max[j]) - ϵ*mpc.con.C_ymax[j] + elseif i ≤ 2nŶ + nx̂ + j = i - 2nŶ + g[i] = (mpc.con.x̂0min[j] - x̂0end[j]) - ϵ*mpc.con.c_x̂min[j] + elseif i ≤ 2nŶ + 2nx̂ + j = i - 2nŶ - nx̂ + g[i] = (x̂0end[j] - mpc.con.x̂0max[j]) - ϵ*mpc.con.c_x̂max[j] + else + j = i - 2nŶ - 2nx̂ + g[i] = gc[j] + end + end + return g +end """ con_nonlinprogeq!( From 7dbb2c7fb6278b9a5062cc82120f6560da14f0bd Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 16 Mar 2025 18:55:26 -0400 Subject: [PATCH 2/5] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 0927b31f..52915e11 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.3" +version = "1.4.4" [deps] LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" From f66d49ef01995162c30cfc988e5d16f7ff614f6b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 16 Mar 2025 19:21:17 -0400 Subject: [PATCH 3/5] test: improve coverage --- src/controller/transcription.jl | 2 +- test/3_test_predictive_control.jl | 25 ++++++++++++++++++++----- 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index fdb7fb00..65737bd7 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1158,7 +1158,7 @@ function con_nonlinprog!( j = i - nŶ g[i] = (Ŷ0[j] - mpc.con.Y0max[j]) - ϵ*mpc.con.C_ymax[j] else - j = i - 2nŶ - 2nx̂ + j = i - 2nŶ g[i] = gc[j] end end diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 0d30cb9f..fc969529 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, Hc=5, gc=gc, nc=2Hp, p=[0; 0]) - + nmpc_lin = NonLinMPC(linmodel; Hp, Hc=5, 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, Hc=5, gc=gc, nc=2Hp, p=[0; 0]) + nmpc = NonLinMPC(nonlinmodel; Hp, Hc=5, gc, nc=2Hp, p=[0; 0]) setconstraint!(nmpc, x̂min=[-1e6,-Inf], x̂max=[+1e6,+Inf]) setconstraint!(nmpc, umin=[-1e6], umax=[+1e6]) @@ -1000,9 +1000,11 @@ end moveinput!(nmpc, [100]) 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)) + @test all(isapprox.(info[:gc][Hp+1:end], 0.0; atol=1e-1))=# - nmpc_ms = NonLinMPC(nonlinmodel; Hp, Hc=5, transcription=MultipleShooting()) + nmpc_ms = NonLinMPC( + nonlinmodel; Hp, Hc=5, transcription=MultipleShooting(), gc, nc=2Hp, p=[0; 0] + ) preparestate!(nmpc_ms, [0]) @@ -1013,6 +1015,19 @@ end moveinput!(nmpc_ms, [10]) info = getinfo(nmpc_ms) @test info[:x̂end][1] ≈ 0 atol=1e-1 + setconstraint!(nmpc_ms, x̂min=[-1e6,-Inf], x̂max=[1e6,+Inf]) + + nmpc_ms.p .= [1; 0] + moveinput!(nmpc_ms, [100]) + info = getinfo(nmpc_ms) + @test all(isapprox.(info[:U], 4.2; atol=1e-1)) + @test all(isapprox.(info[:gc][1:Hp], 0.0; atol=1e-1)) + + nmpc_ms.p .= [0; 1] + moveinput!(nmpc_ms, [100]) + info = getinfo(nmpc_ms) + @test all(isapprox.(info[:Ŷ], 3.14; atol=1e-1)) + @test all(isapprox.(info[:gc][Hp+1:end], 0.0; atol=1e-1)) end From 1a3992ab429b204a9d2e1130da5f33563573625b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 16 Mar 2025 19:24:56 -0400 Subject: [PATCH 4/5] test: remove trailing `=#` --- test/3_test_predictive_control.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index fc969529..bee332cb 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -1000,7 +1000,7 @@ end moveinput!(nmpc, [100]) 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))=# + @test all(isapprox.(info[:gc][Hp+1:end], 0.0; atol=1e-1)) nmpc_ms = NonLinMPC( nonlinmodel; Hp, Hc=5, transcription=MultipleShooting(), gc, nc=2Hp, p=[0; 0] From aef8fe1286241d8433a3eb2ee2f3cf4db20cd42c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 16 Mar 2025 20:12:19 -0400 Subject: [PATCH 5/5] test: improve coverage --- test/3_test_predictive_control.jl | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index bee332cb..d099cb6b 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -1001,13 +1001,24 @@ 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(), gc, nc=2Hp, p=[0; 0] ) + setconstraint!(nmpc_ms, x̂min=[-1e6,-Inf], x̂max=[+1e6,+Inf]) + setconstraint!(nmpc_ms, ymin=[-100], ymax=[100]) preparestate!(nmpc_ms, [0]) + setconstraint!(nmpc_ms, ymin=[-0.5], ymax=[0.9]) + moveinput!(nmpc_ms, [-100]) + info = getinfo(nmpc_ms) + @test all(isapprox.(info[:Ŷ], -0.5; atol=1e-1)) + moveinput!(nmpc_ms, [100]) + info = getinfo(nmpc_ms) + @test all(isapprox.(info[:Ŷ], 0.9; atol=1e-1)) + setconstraint!(nmpc_ms, ymin=[-100], ymax=[100]) + setconstraint!(nmpc_ms, x̂min=[-1e-6,-Inf], x̂max=[+1e-6,+Inf]) moveinput!(nmpc_ms, [-10]) info = getinfo(nmpc_ms)