Skip to content

Commit c12d7bc

Browse files
franckgagafranckgaga
authored
Merge pull request #178 from JuliaControl/debug_ms_terminal_2
Debug: ineq. constraint for `NonLinModel` and `MultipleShooting`
2 parents b84d375 + aef8fe1 commit c12d7bc

4 files changed

Lines changed: 117 additions & 54 deletions

File tree

‎Project.toml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
name = "ModelPredictiveControl"
22
uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
33
authors = ["Francis Gagnon"]
4-
version = "1.4.3"
4+
version = "1.4.4"
55

66
[deps]
77
LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"

‎src/controller/nonlinmpc.jl‎

Lines changed: 1 addition & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -582,7 +582,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
582582
Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃)
583583
Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0)
584584
gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ)
585-
g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ)
585+
g = con_nonlinprog!(g, mpc, model, transcription, x̂0end, Ŷ0, gc, ϵ)
586586
geq = con_nonlinprogeq!(geq, X̂0, Û0, mpc, model, transcription, U0, Z̃)
587587
end
588588
return nothing
@@ -692,53 +692,6 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
692692
return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs!
693693
end
694694

695-
"""
696-
con_nonlinprog!(g, mpc::NonLinMPC, model::LinModel, _ , _ , gc, ϵ) -> g
697-
698-
Nonlinear constrains for [`NonLinMPC`](@ref) when `model` is a [`LinModel`](@ref).
699-
700-
The method mutates the `g` vectors in argument and returns it. Only the custom constraints
701-
are include in the `g` vector.
702-
"""
703-
function con_nonlinprog!(g, mpc::NonLinMPC, ::LinModel, _ , _ , gc, ϵ)
704-
for i in eachindex(g)
705-
g[i] = gc[i]
706-
end
707-
return g
708-
end
709-
710-
"""
711-
con_nonlinprog!(g, mpc::NonLinMPC, model::SimModel, x̂0end, Ŷ0, gc, ϵ) -> g
712-
713-
Nonlinear constrains for [`NonLinMPC`](@ref) when `model` is not a [`LinModel`](@ref).
714-
715-
The method mutates the `g` vectors in argument and returns it. The output prediction,
716-
the terminal state and the custom constraints are include in the `g` vector.
717-
"""
718-
function con_nonlinprog!(g, mpc::NonLinMPC, ::SimModel, x̂0end, Ŷ0, gc, ϵ)
719-
nx̂, nŶ = length(x̂0end), length(Ŷ0)
720-
for i in eachindex(g)
721-
mpc.con.i_g[i] || continue
722-
if i ≤ nŶ
723-
j = i
724-
g[i] = (mpc.con.Y0min[j] - Ŷ0[j]) - ϵ*mpc.con.C_ymin[j]
725-
elseif i ≤ 2nŶ
726-
j = i - nŶ
727-
g[i] = (Ŷ0[j] - mpc.con.Y0max[j]) - ϵ*mpc.con.C_ymax[j]
728-
elseif i ≤ 2nŶ + nx̂
729-
j = i - 2nŶ
730-
g[i] = (mpc.con.x̂0min[j] - x̂0end[j]) - ϵ*mpc.con.c_x̂min[j]
731-
elseif i ≤ 2nŶ + 2nx̂
732-
j = i - 2nŶ - nx̂
733-
g[i] = (x̂0end[j] - mpc.con.x̂0max[j]) - ϵ*mpc.con.c_x̂max[j]
734-
else
735-
j = i - 2nŶ - 2nx̂
736-
g[i] = gc[j]
737-
end
738-
end
739-
return g
740-
end
741-
742695
@doc raw"""
743696
con_custom!(gc, mpc::NonLinMPC, Ue, Ŷe, ϵ) -> gc
744697

‎src/controller/transcription.jl‎

Lines changed: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1116,6 +1116,90 @@ function predict!(
11161116
return Ŷ0, x̂0end
11171117
end
11181118

1119+
"""
1120+
con_nonlinprog!(
1121+
g, mpc::PredictiveController, model::LinModel, ::TranscriptionMethod, _ , _ , gc, ϵ
1122+
) -> g
1123+
1124+
Nonlinear constrains when `model` is a [`LinModel`](@ref).
1125+
1126+
The method mutates the `g` vectors in argument and returns it. Only the custom constraints
1127+
are include in the `g` vector.
1128+
"""
1129+
function con_nonlinprog!(
1130+
g, mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , _ , gc, ϵ
1131+
)
1132+
for i in eachindex(g)
1133+
g[i] = gc[i]
1134+
end
1135+
return g
1136+
end
1137+
1138+
"""
1139+
con_nonlinprog!(
1140+
g, mpc::PredictiveController, model::NonLinModel, ::TranscriptionMethod, x̂0end, Ŷ0, gc, ϵ
1141+
) -> g
1142+
1143+
Nonlinear constrains when `model` is a [`NonLinModel`](@ref) with non-[`SingleShooting`](@ref).
1144+
1145+
The method mutates the `g` vectors in argument and returns it. The output prediction and the
1146+
custom constraints are include in the `g` vector.
1147+
"""
1148+
function con_nonlinprog!(
1149+
g, mpc::PredictiveController, ::NonLinModel, ::TranscriptionMethod, x̂0end, Ŷ0, gc, ϵ
1150+
)
1151+
nx̂, nŶ = length(x̂0end), length(Ŷ0)
1152+
for i in eachindex(g)
1153+
mpc.con.i_g[i] || continue
1154+
if i ≤ nŶ
1155+
j = i
1156+
g[i] = (mpc.con.Y0min[j] - Ŷ0[j]) - ϵ*mpc.con.C_ymin[j]
1157+
elseif i ≤ 2nŶ
1158+
j = i - nŶ
1159+
g[i] = (Ŷ0[j] - mpc.con.Y0max[j]) - ϵ*mpc.con.C_ymax[j]
1160+
else
1161+
j = i - 2nŶ
1162+
g[i] = gc[j]
1163+
end
1164+
end
1165+
return g
1166+
end
1167+
1168+
"""
1169+
con_nonlinprog!(
1170+
g, mpc::PredictiveController, model::NonLinModel, ::SingleShooting, x̂0end, Ŷ0, gc, ϵ
1171+
) -> g
1172+
1173+
Nonlinear constrains when `model` is [`NonLinModel`](@ref) with [`SingleShooting`](@ref).
1174+
1175+
The method mutates the `g` vectors in argument and returns it. The output prediction,
1176+
the terminal state and the custom constraints are include in the `g` vector.
1177+
"""
1178+
function con_nonlinprog!(
1179+
g, mpc::PredictiveController, ::NonLinModel, ::SingleShooting, x̂0end, Ŷ0, gc, ϵ
1180+
)
1181+
nx̂, nŶ = length(x̂0end), length(Ŷ0)
1182+
for i in eachindex(g)
1183+
mpc.con.i_g[i] || continue
1184+
if i ≤ nŶ
1185+
j = i
1186+
g[i] = (mpc.con.Y0min[j] - Ŷ0[j]) - ϵ*mpc.con.C_ymin[j]
1187+
elseif i ≤ 2nŶ
1188+
j = i - nŶ
1189+
g[i] = (Ŷ0[j] - mpc.con.Y0max[j]) - ϵ*mpc.con.C_ymax[j]
1190+
elseif i ≤ 2nŶ + nx̂
1191+
j = i - 2nŶ
1192+
g[i] = (mpc.con.x̂0min[j] - x̂0end[j]) - ϵ*mpc.con.c_x̂min[j]
1193+
elseif i ≤ 2nŶ + 2nx̂
1194+
j = i - 2nŶ - nx̂
1195+
g[i] = (x̂0end[j] - mpc.con.x̂0max[j]) - ϵ*mpc.con.c_x̂max[j]
1196+
else
1197+
j = i - 2nŶ - 2nx̂
1198+
g[i] = gc[j]
1199+
end
1200+
end
1201+
return g
1202+
end
11191203

11201204
"""
11211205
con_nonlinprogeq!(

‎test/3_test_predictive_control.jl‎

Lines changed: 31 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -865,8 +865,8 @@ end
865865
Hp=50
866866

867867
linmodel = LinModel(tf([2], [10000, 1]), 3000.0)
868-
nmpc_lin = NonLinMPC(linmodel; Hp, Hc=5, gc=gc, nc=2Hp, p=[0; 0])
869-
868+
nmpc_lin = NonLinMPC(linmodel; Hp, Hc=5, gc, nc=2Hp, p=[0; 0])
869+
870870
setconstraint!(nmpc_lin, x̂min=[-1e6,-Inf], x̂max=[1e6,+Inf])
871871
setconstraint!(nmpc_lin, umin=[-10], umax=[10])
872872
setconstraint!(nmpc_lin, Δumin=[-1e6], Δumax=[1e6])
@@ -935,7 +935,7 @@ end
935935
f = (x,u,_,p) -> p.A*x + p.Bu*u
936936
h = (x,_,p) -> p.C*x
937937
nonlinmodel = NonLinModel(f, h, linmodel.Ts, 1, 1, 1, solver=nothing, p=linmodel)
938-
nmpc = NonLinMPC(nonlinmodel; Hp, Hc=5, gc=gc, nc=2Hp, p=[0; 0])
938+
nmpc = NonLinMPC(nonlinmodel; Hp, Hc=5, gc, nc=2Hp, p=[0; 0])
939939

940940
setconstraint!(nmpc, x̂min=[-1e6,-Inf], x̂max=[+1e6,+Inf])
941941
setconstraint!(nmpc, umin=[-1e6], umax=[+1e6])
@@ -1001,18 +1001,44 @@ end
10011001
info = getinfo(nmpc)
10021002
@test all(isapprox.(info[:Ŷ], 3.14; atol=1e-1))
10031003
@test all(isapprox.(info[:gc][Hp+1:end], 0.0; atol=1e-1))
1004-
1005-
nmpc_ms = NonLinMPC(nonlinmodel; Hp, Hc=5, transcription=MultipleShooting())
10061004

1005+
nmpc_ms = NonLinMPC(
1006+
nonlinmodel; Hp, Hc=5, transcription=MultipleShooting(), gc, nc=2Hp, p=[0; 0]
1007+
)
1008+
1009+
setconstraint!(nmpc_ms, x̂min=[-1e6,-Inf], x̂max=[+1e6,+Inf])
1010+
setconstraint!(nmpc_ms, ymin=[-100], ymax=[100])
10071011
preparestate!(nmpc_ms, [0])
10081012

1013+
setconstraint!(nmpc_ms, ymin=[-0.5], ymax=[0.9])
1014+
moveinput!(nmpc_ms, [-100])
1015+
info = getinfo(nmpc_ms)
1016+
@test all(isapprox.(info[:Ŷ], -0.5; atol=1e-1))
1017+
moveinput!(nmpc_ms, [100])
1018+
info = getinfo(nmpc_ms)
1019+
@test all(isapprox.(info[:Ŷ], 0.9; atol=1e-1))
1020+
setconstraint!(nmpc_ms, ymin=[-100], ymax=[100])
1021+
10091022
setconstraint!(nmpc_ms, x̂min=[-1e-6,-Inf], x̂max=[+1e-6,+Inf])
10101023
moveinput!(nmpc_ms, [-10])
10111024
info = getinfo(nmpc_ms)
10121025
@test info[:x̂end][1] ≈ 0 atol=1e-1
10131026
moveinput!(nmpc_ms, [10])
10141027
info = getinfo(nmpc_ms)
10151028
@test info[:x̂end][1] ≈ 0 atol=1e-1
1029+
setconstraint!(nmpc_ms, x̂min=[-1e6,-Inf], x̂max=[1e6,+Inf])
1030+
1031+
nmpc_ms.p .= [1; 0]
1032+
moveinput!(nmpc_ms, [100])
1033+
info = getinfo(nmpc_ms)
1034+
@test all(isapprox.(info[:U], 4.2; atol=1e-1))
1035+
@test all(isapprox.(info[:gc][1:Hp], 0.0; atol=1e-1))
1036+
1037+
nmpc_ms.p .= [0; 1]
1038+
moveinput!(nmpc_ms, [100])
1039+
info = getinfo(nmpc_ms)
1040+
@test all(isapprox.(info[:Ŷ], 3.14; atol=1e-1))
1041+
@test all(isapprox.(info[:gc][Hp+1:end], 0.0; atol=1e-1))
10161042

10171043
end
10181044

0 commit comments

Comments
 (0)