Skip to content

Commit 547d2f2

Browse files
franckgagafranckgaga
committed
tests: simple constraint violation testing with custom function gc 
1 parent 6980f69 commit 547d2f2

File tree

1 file changed

+76
-12
lines changed

1 file changed

+76
-12
lines changed

‎test/test_predictive_control.jl‎

Lines changed: 76 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -230,13 +230,20 @@ end
230230
info = getinfo(mpc)
231231
@test info[:ΔU][begin] ≈ -1.5 atol=1e-1
232232
@test info[:U][end] ≈ -3 atol=1e-1
233+
moveinput!(mpc, [10])
234+
info = getinfo(mpc)
235+
@test info[:ΔU][begin] ≈ 1.5 atol=1e-1
236+
@test info[:U][end] ≈ 3 atol=1e-1
233237

234238
setconstraint!(mpc, umin=[-10], umax=[10])
235239
setconstraint!(mpc, Δumin=[-15], Δumax=[15])
236240
setconstraint!(mpc, ymin=[-0.5], ymax=[0.5])
237241
moveinput!(mpc, [-10])
238242
info = getinfo(mpc)
239243
@test info[:Ŷ][end] ≈ -0.5 atol=1e-1
244+
moveinput!(mpc, [10])
245+
info = getinfo(mpc)
246+
@test info[:Ŷ][end] ≈ 0.5 atol=1e-1
240247

241248
setconstraint!(mpc, umin=[-10], umax=[10])
242249
setconstraint!(mpc, Δumin=[-15], Δumax=[15])
@@ -245,6 +252,10 @@ end
245252
info = getinfo(mpc)
246253
@test info[:Ŷ][end] ≈ -10 atol=1e-1
247254
@test info[:Ŷ][begin] ≈ -0.5 atol=1e-1
255+
moveinput!(mpc, [10])
256+
info = getinfo(mpc)
257+
@test info[:Ŷ][end] ≈ 10 atol=1e-1
258+
@test info[:Ŷ][begin] ≈ 0.5 atol=1e-1
248259

249260
setconstraint!(mpc, umin=[-1e3], umax=[+1e3])
250261
setconstraint!(mpc, Δumin=[-1e3], Δumax=[+1e3])
@@ -253,6 +264,9 @@ end
253264
moveinput!(mpc, [-10])
254265
info = getinfo(mpc)
255266
@test info[:x̂end][1] ≈ 0 atol=1e-1
267+
moveinput!(mpc, [10])
268+
info = getinfo(mpc)
269+
@test info[:x̂end][1] ≈ 0 atol=1e-1
256270
end
257271

258272
@testset "LinMPC terminal cost" begin
@@ -700,8 +714,10 @@ end
700714
end
701715

702716
@testset "NonLinMPC constraint violation" begin
717+
gc( _ , Ŷe, _ , p , ϵ) = p[]*(Ŷe .- 3.14 .- ϵ)
718+
703719
linmodel = LinModel(tf([2], [10000, 1]), 3000.0)
704-
nmpc_lin = NonLinMPC(linmodel, Hp=50, Hc=5)
720+
nmpc_lin = NonLinMPC(linmodel, Hp=50, Hc=5, gc=gc, nc=50+1, p=[0])
705721

706722
setconstraint!(nmpc_lin, x̂min=[-1e3,-Inf], x̂max=[1e3,+Inf])
707723
setconstraint!(nmpc_lin, umin=[-3], umax=[3])
@@ -710,23 +726,34 @@ end
710726
preparestate!(nmpc_lin, [0])
711727
moveinput!(nmpc_lin, [-20])
712728
info = getinfo(nmpc_lin)
713-
@test info[:ΔU][begin] ≈ -1.5 atol=1e-2
714-
@test info[:U][end] ≈ -3 atol=1e-2
729+
@test info[:ΔU][begin] ≈ -1.5 atol=1e-1
730+
@test info[:U][end] ≈ -3 atol=1e-1
731+
moveinput!(nmpc_lin, [20])
732+
info = getinfo(nmpc_lin)
733+
@test info[:ΔU][begin] ≈ 1.5 atol=1e-1
734+
@test info[:U][end] ≈ 3 atol=1e-1
715735

716736
setconstraint!(nmpc_lin, umin=[-10], umax=[10])
717737
setconstraint!(nmpc_lin, Δumin=[-15], Δumax=[15])
718738
setconstraint!(nmpc_lin, ymin=[-0.5], ymax=[0.5])
719739
moveinput!(nmpc_lin, [-20])
720740
info = getinfo(nmpc_lin)
721-
@test info[:Ŷ][end] ≈ -0.5 atol=1e-2
741+
@test info[:Ŷ][end] ≈ -0.5 atol=1e-1
742+
moveinput!(nmpc_lin, [20])
743+
info = getinfo(nmpc_lin)
744+
@test info[:Ŷ][end] ≈ 0.5 atol=1e-1
722745

723746
setconstraint!(nmpc_lin, umin=[-10], umax=[10])
724747
setconstraint!(nmpc_lin, Δumin=[-15], Δumax=[15])
725748
setconstraint!(nmpc_lin, Ymin=[-0.5; fill(-100, 49)], Ymax=[0.5; fill(+100, 49)])
726749
moveinput!(nmpc_lin, [-10])
727750
info = getinfo(nmpc_lin)
728-
@test info[:Ŷ][end] ≈ -10 atol=1e-2
729-
@test info[:Ŷ][begin] ≈ -0.5 atol=1e-2
751+
@test info[:Ŷ][end] ≈ -10 atol=1e-1
752+
@test info[:Ŷ][begin] ≈ -0.5 atol=1e-1
753+
moveinput!(nmpc_lin, [10])
754+
info = getinfo(nmpc_lin)
755+
@test info[:Ŷ][end] ≈ 10 atol=1e-1
756+
@test info[:Ŷ][begin] ≈ 0.5 atol=1e-1
730757

731758
setconstraint!(nmpc_lin, umin=[-1e3], umax=[+1e3])
732759
setconstraint!(nmpc_lin, Δumin=[-1e3], Δumax=[+1e3])
@@ -735,11 +762,24 @@ end
735762
moveinput!(nmpc_lin, [-10])
736763
info = getinfo(nmpc_lin)
737764
@test info[:x̂end][1] ≈ 0 atol=1e-1
765+
moveinput!(nmpc_lin, [10])
766+
info = getinfo(nmpc_lin)
767+
@test info[:x̂end][1] ≈ 0 atol=1e-1
768+
769+
nmpc_lin.p[] = 1
770+
setconstraint!(nmpc_lin, x̂min=[-1e3,-Inf], x̂max=[1e3,+Inf])
771+
setconstraint!(nmpc_lin, umin=[-10], umax=[10])
772+
setconstraint!(nmpc_lin, Δumin=[-15], Δumax=[15])
773+
setconstraint!(nmpc_lin, ymin=[-100], ymax=[100])
774+
moveinput!(nmpc_lin, [20])
775+
info = getinfo(nmpc_lin)
776+
@test info[:Ŷ][end] ≈ 3.14 atol=1e-1
777+
@test info[:Ŷ][begin] ≈ 3.14 atol=1e-1
738778

739779
f = (x,u,_,_) -> linmodel.A*x + linmodel.Bu*u
740780
h = (x,_,_) -> linmodel.C*x
741781
nonlinmodel = NonLinModel(f, h, linmodel.Ts, 1, 1, 1, solver=nothing)
742-
nmpc = NonLinMPC(nonlinmodel, Hp=50, Hc=5)
782+
nmpc = NonLinMPC(nonlinmodel, Hp=50, Hc=5, gc=gc, nc=50+1, p=[0])
743783

744784
setconstraint!(nmpc, x̂min=[-1e3,-Inf], x̂max=[1e3,+Inf])
745785
setconstraint!(nmpc, umin=[-3], umax=[3])
@@ -748,23 +788,34 @@ end
748788
preparestate!(nmpc, [0])
749789
moveinput!(nmpc, [-20])
750790
info = getinfo(nmpc)
751-
@test info[:ΔU][begin] ≈ -1.5 atol=1e-2
752-
@test info[:U][end] ≈ -3 atol=1e-2
791+
@test info[:ΔU][begin] ≈ -1.5 atol=1e-1
792+
@test info[:U][end] ≈ -3 atol=1e-1
793+
moveinput!(nmpc, [20])
794+
info = getinfo(nmpc)
795+
@test info[:ΔU][begin] ≈ 1.5 atol=1e-1
796+
@test info[:U][end] ≈ 3 atol=1e-1
753797

754798
setconstraint!(nmpc, umin=[-10], umax=[10])
755799
setconstraint!(nmpc, Δumin=[-15], Δumax=[15])
756800
setconstraint!(nmpc, ymin=[-0.5], ymax=[0.5])
757801
moveinput!(nmpc, [-20])
758802
info = getinfo(nmpc)
759-
@test info[:Ŷ][end] ≈ -0.5 atol=1e-2
803+
@test info[:Ŷ][end] ≈ -0.5 atol=1e-1
804+
moveinput!(nmpc, [20])
805+
info = getinfo(nmpc)
806+
@test info[:Ŷ][end] ≈ 0.5 atol=1e-1
760807

761808
setconstraint!(nmpc, umin=[-10], umax=[10])
762809
setconstraint!(nmpc, Δumin=[-15], Δumax=[15])
763810
setconstraint!(nmpc, Ymin=[-0.5; fill(-100, 49)], Ymax=[0.5; fill(+100, 49)])
764811
moveinput!(nmpc, [-10])
765812
info = getinfo(nmpc)
766-
@test info[:Ŷ][end] ≈ -10 atol=1e-2
767-
@test info[:Ŷ][begin] ≈ -0.5 atol=1e-2
813+
@test info[:Ŷ][end] ≈ -10 atol=1e-1
814+
@test info[:Ŷ][begin] ≈ -0.5 atol=1e-1
815+
moveinput!(nmpc, [10])
816+
info = getinfo(nmpc)
817+
@test info[:Ŷ][end] ≈ 10 atol=1e-1
818+
@test info[:Ŷ][begin] ≈ 0.5 atol=1e-1
768819

769820
setconstraint!(nmpc, umin=[-1e3], umax=[+1e3])
770821
setconstraint!(nmpc, Δumin=[-1e3], Δumax=[+1e3])
@@ -773,6 +824,19 @@ end
773824
moveinput!(nmpc, [-10])
774825
info = getinfo(nmpc)
775826
@test info[:x̂end][1] ≈ 0 atol=1e-1
827+
moveinput!(nmpc, [10])
828+
info = getinfo(nmpc)
829+
@test info[:x̂end][1] ≈ 0 atol=1e-1
830+
831+
nmpc.p[] = 1
832+
setconstraint!(nmpc, x̂min=[-1e3,-Inf], x̂max=[1e3,+Inf])
833+
setconstraint!(nmpc, umin=[-10], umax=[10])
834+
setconstraint!(nmpc, Δumin=[-15], Δumax=[15])
835+
setconstraint!(nmpc, ymin=[-100], ymax=[100])
836+
moveinput!(nmpc, [20])
837+
info = getinfo(nmpc)
838+
@test info[:Ŷ][end] ≈ 3.14 atol=1e-1
839+
@test info[:Ŷ][begin] ≈ 3.14 atol=1e-1
776840
end
777841

778842
@testset "NonLinMPC set model" begin

0 commit comments

Comments
 (0)