Merge pull request #178 from JuliaControl/debug_ms_terminal_2

Debug: ineq. constraint for `NonLinModel` and `MultipleShooting`

Author: franckgaga

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"

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
 

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Ŷ
+            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!(

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])
@@ -1001,18 +1001,44 @@ 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())
 
+    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)
     @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
+    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