debug memoize function

Author: franckgaga

LocalPreferences.toml

@@ -0,0 +1,2 @@
+[ModelPredictiveControl]
+precompile_workload = false

example/juMPC.jl

@@ -4,7 +4,12 @@ using Pkg
 using Revise
 Pkg.activate(".")
 
+
 using ModelPredictiveControl
+using Preferences
+set_preferences!(ModelPredictiveControl, "precompile_workload" => false; force=true)
+
+
 #using JuMP, DAQP
 #using JuMP, HiGHS
 using JuMP, Ipopt
@@ -71,13 +76,16 @@ nmpc1 = NonLinMPC(uscKalmanFilter1)
 
 nmpc2 = NonLinMPC(nonLinModel2, Hp=15, Hc=1, Mwt=[1, 1] , Nwt=[0.1, 0.1], Cwt=1e5)
 
+
 setconstraint!(nmpc2, c_umin=[0,0], c_umax=[0,0])
 setconstraint!(nmpc2, c_ŷmin=[1,1], c_ŷmax=[1,1])
 setconstraint!(nmpc2, umin=[5, 9.9], umax=[Inf,Inf])
 setconstraint!(nmpc2, ŷmin=[-Inf,-Inf], ŷmax=[55, 35])
 setconstraint!(nmpc2, Δumin=[-Inf,-Inf],Δumax=[+Inf,+Inf])
 
 
+
+
 nx = linModel4.nx
 kf = KalmanFilter(linModel4, σP0=10*ones(nx), σQ=0.01*ones(nx), σR=[0.1, 0.1], σQ_int=0.05*ones(2), σP0_int=10*ones(2))
 
@@ -97,6 +105,7 @@ setconstraint!(nmpc, umin=[5, 9.9], umax=[Inf,Inf])
 setconstraint!(nmpc, ŷmin=[-Inf,-Inf], ŷmax=[55, 35])
 setconstraint!(nmpc, Δumin=[-Inf,-Inf],Δumax=[+Inf,+Inf])
 
+
 function test_mpc(model, mpc)
     N = 200 
     u_data = zeros(2,N)
@@ -133,9 +142,9 @@ function test_mpc(model, mpc)
     return u_data, y_data, r_data, d_data
 end
 
-#@time u_data, y_data, r_data, d_data = test_mpc(linModel4, mpc)
+@time u_data, y_data, r_data, d_data = test_mpc(linModel4, mpc)
 
-#@time u_data, y_data, r_data, d_data = test_mpc(linModel4, nmpc)
+@time u_data, y_data, r_data, d_data = test_mpc(linModel4, nmpc)
 
 @time u_data, y_data, r_data, d_data = test_mpc(nonLinModel2, nmpc2)
 
@@ -160,5 +169,3 @@ pd = plot(0:N-1,d_data[1,:],label=raw"$d_1$")
 display(pd)
 display(pu)
 display(py)
-
-

src/controller/linmpc.jl

@@ -198,6 +198,8 @@ function LinMPC(
     return LinMPC{S}(estim, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru, optim)
 end
 
+setnontlincon!(mpc::LinMPC, model::LinModel) = nothing
+
 function init_objective!(mpc::LinMPC, ΔŨ)
     set_objective_function(mpc.optim, obj_quadprog(ΔŨ, mpc.P̃, mpc.q̃))
     return nothing

src/controller/nonlinmpc.jl

@@ -80,10 +80,13 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
         nonlinconstraint = let mpc=mpc, model=model # capture mpc and model variables
             (ΔŨ...) -> con_nonlinprog(mpc, model, ΔŨ)
         end
-        register(mpc.optim, :nonlinconstraint, nvar, nonlinconstraint, autodiff=true)
-        ncon = sum(con.i_Ŷmin) + sum(con.i_Ŷmax)
-        @NLconstraint(mpc.optim, [i=1:ncon], nonlinconstraint(ΔŨ...) ≤ zeros(ncon))
-
+        nonlincon_memoized = memoize(nonlinconstraint, 2*ny*Hp)
+        for i=1:ny*Hp
+            register(mpc.optim, Symbol("C_Ŷmin_$(i)"), nvar, nonlincon_memoized[i], autodiff=true)
+        end
+        for i=1:ny*Hp
+            register(mpc.optim, Symbol("C_Ŷmax_$(i)"), nvar, nonlincon_memoized[ny*Hp+i], autodiff=true)
+        end
         set_silent(optim)
         return mpc
     end
@@ -193,6 +196,23 @@ function NonLinMPC(
     return NonLinMPC{S, JEFunc}(estim, Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, ru, optim)
 end
 
+setnontlincon!(mpc::NonLinMPC, model::LinModel) = nothing
+
+function setnonlincon!(mpc::NonLinMPC, model::NonLinModel)
+    optim = mpc.optim
+    ΔŨ = mpc.optim[:ΔŨ]
+    con = mpc.con
+    map(con -> delete(optim, con), all_nonlinear_constraints(optim))
+    for i in findall(con.i_Ŷmin)
+        f_sym = Symbol("C_Ŷmin_$(i)")
+        add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨ...)) <= 0))
+    end
+    for i in findall(con.i_Ŷmax)
+        f_sym = Symbol("C_Ŷmax_$(i)")
+        add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨ...)) <= 0))
+    end
+    return nothing
+end
 
 init_objective!(mpc::NonLinMPC, _ ) = nothing
 
@@ -236,11 +256,12 @@ function con_nonlinprog(mpc::NonLinMPC, model::SimModel, ΔŨ::NTuple{N, T}) wh
     ΔŨ = collect(ΔŨ) # convert NTuple to Vector
     U0 = mpc.S̃_Hp*ΔŨ + mpc.T_Hp*(mpc.estim.lastu0)
     Ŷ = evalŶ(mpc, model, mpc.x̂d, mpc.d0, mpc.D̂0, U0)
-    C_Ŷmin = (mpc.con.Ŷmin - Ŷ)[mpc.con.i_Ŷmin]
-    C_Ŷmax = (Ŷ - mpc.con.Ŷmax)[mpc.con.i_Ŷmin]
+    C_Ŷmin = (mpc.con.Ŷmin - Ŷ)
+    C_Ŷmax = (Ŷ - mpc.con.Ŷmax)
     if !isinf(mpc.C) # constraint softening activated :
-        C_Ŷmin = C_Ŷmin - ΔŨ[end]*mpc.con.c_Ŷmin[mpc.con.i_Ŷmin]
-        C_Ŷmax = C_Ŷmax - ΔŨ[end]*mpc.con.c_Ŷmin[mpc.con.i_Ŷmin]
+        ϵ = ΔŨ[end]
+        C_Ŷmin = C_Ŷmin - ϵ*mpc.con.c_Ŷmin
+        C_Ŷmax = C_Ŷmax - ϵ*mpc.con.c_Ŷmin
     end
     C = [C_Ŷmin; C_Ŷmax]
     return C
@@ -258,3 +279,32 @@ function evalŶ(mpc, model, x̂d, d0, D̂0, U0::Vector{T}) where {T}
     return Ŷd0 + mpc.F
 end
 
+"""
+    memoize(myfunc::Function, n_outputs::Int)
+
+Take a function `myfunc` and return a vector of length `n_outputs`, where element
+`i` is a function that returns the equivalent of `myfunc(x...)[i]`.
+
+To avoid duplication of work, cache the most-recent evaluations of `myfunc`.
+Because `myfunc_i` is auto-differentiated with ForwardDiff, our cache needs to
+work when `x` is a `Float64` and a `ForwardDiff.Dual`.
+"""
+function memoize(f::Function, n_outputs::Int)
+    last_ΔŨ , last_f = nothing, nothing
+    function f_i(i, ΔŨ::Float64...)
+        if ΔŨ !== last_ΔŨ
+            last_f = f(ΔŨ...)
+            last_ΔŨ = ΔŨ
+        end
+        return last_f[i]
+    end
+    last_dΔŨ, last_dfdΔŨ = nothing, nothing
+    function f_i(i, dΔŨ::T...) where {T<:Real}
+        if dΔŨ !== last_dΔŨ
+            last_dfdΔŨ = f(dΔŨ...)
+            last_dΔŨ = dΔŨ
+        end
+        return last_dfdΔŨ[i]
+    end
+    return [(x...) -> f_i(i, x...) for i in 1:n_outputs]
+end

src/predictive_control.jl

@@ -184,9 +184,12 @@ function setconstraint!(
     delete(mpc.optim, mpc.optim[:linconstraint])
     unregister(mpc.optim, :linconstraint)
     @constraint(mpc.optim, linconstraint, A*ΔŨ .≤ b)
+    setnonlincon!(mpc, model)
     return mpc
 end
 
+setnonlincon!(::PredictiveController, ::SimModel) = nothing
+
 @doc raw"""
     moveinput!(
         mpc::PredictiveController, 
@@ -639,7 +642,7 @@ function init_defaultcon(model, Hp, Hc, C, S_Hp, S_Hc, N_Hc, E)
     nu, ny = model.nu, model.ny
     umin,       umax    = fill(-Inf, nu), fill(+Inf, nu)
     Δumin,      Δumax   = fill(-Inf, nu), fill(+Inf, nu)
-    ŷmin,       ŷmax    = fill(-Inf, ny), fill(+Inf, ny)
+    ŷmin,       ŷmax    = fill(-999, ny), fill(+999, ny)
     c_umin,     c_umax  = fill(0.0, nu),  fill(0.0, nu)
     c_Δumin,    c_Δumax = fill(0.0, nu),  fill(0.0, nu)
     c_ŷmin,     c_ŷmax  = fill(1.0, ny),  fill(1.0, ny)