Merge pull request #46 from JuliaControl/nlp_single_funcs

franckgaga · web-flow · commit 4e6b0c2a0859 · 2024-04-08T13:50:00.000-04:00
Changed: avoid `collect` in `NonLinMPC` and `MovingHorizonEstimator` objective and constraints
diff --git a/docs/src/manual/nonlinmpc.md b/docs/src/manual/nonlinmpc.md
@@ -87,7 +87,7 @@ The vectors `σQ` and σR `σR` are the standard deviations of the process and s
 respectively. The value for the velocity ``ω`` is higher here (`σQ` second value) since
 ``\dot{ω}(t)`` equation includes an uncertain parameter: the friction coefficient ``K``.
 Also, the argument `nint_u` explicitly adds one integrating state at the model input, the
-motor torque ``τ`` , with an associated standard deviation `σQint_u` of 0.1 N m. The
+motor torque ``τ``, with an associated standard deviation `σQint_u` of 0.1 N m. The
 estimator tuning is tested on a plant with a 25 % larger friction coefficient ``K``:
 
 ```@example 1
@@ -108,12 +108,12 @@ As the motor torque is limited to -1.5 to 1.5 N m, we incorporate the input cons
 a [`NonLinMPC`](@ref):
 
 ```@example 1
-nmpc = NonLinMPC(estim, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5])
+nmpc = NonLinMPC(estim, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5], Cwt=Inf)
 nmpc = setconstraint!(nmpc, umin=[-1.5], umax=[+1.5])
 ```
 
-We test `mpc` performance on `plant` by imposing an angular setpoint of 180° (inverted
-position):
+The option `Cwt=Inf` disables constraint softening. We test `mpc` performance on `plant` by
+imposing an angular setpoint of 180° (inverted position):
 
 ```@example 1
 using Logging; disable_logging(Warn)            # hide
@@ -185,7 +185,7 @@ function JE(UE, ŶE, _ )
     τ, ω = UE[1:end-1], ŶE[2:2:end-1]
     return Ts*sum(τ.*ω)
 end
-empc = NonLinMPC(estim2, Hp=20, Hc=2, Mwt=[0.5, 0], Nwt=[2.5], Ewt=3.5e3, JE=JE)
+empc = NonLinMPC(estim2, Hp=20, Hc=2, Mwt=[0.5, 0], Nwt=[2.5], Cwt=Inf, Ewt=3.5e3, JE=JE)
 empc = setconstraint!(empc, umin=[-1.5], umax=[+1.5])
 ```
 
@@ -250,7 +250,7 @@ A [`SteadyKalmanFilter`](@ref) and a [`LinMPC`](@ref) are designed from `linmode
 
 ```@example 1
 kf  = SteadyKalmanFilter(linmodel; σQ, σR, nint_u, σQint_u)
-mpc = LinMPC(kf, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5])
+mpc = LinMPC(kf, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5], Cwt=Inf)
 mpc = setconstraint!(mpc, umin=[-1.5], umax=[+1.5])
 ```
 
@@ -288,7 +288,7 @@ Constructing a [`LinMPC`](@ref) with `DAQP`:
 ```@example 1
 using JuMP, DAQP
 daqp = Model(DAQP.Optimizer, add_bridges=false)
-mpc2 = LinMPC(kf, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5], optim=daqp)
+mpc2 = LinMPC(kf, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5], Cwt=Inf, optim=daqp)
 mpc2 = setconstraint!(mpc2, umin=[-1.5], umax=[+1.5])
 ```
 
diff --git a/src/controller/execute.jl b/src/controller/execute.jl
@@ -445,7 +445,6 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real}
             rethrow(err)
         end
     end
-    ΔŨcurr, ΔŨlast = value.(ΔŨvar), ΔŨ0
     if !issolved(optim)
         status = termination_status(optim)
         if iserror(optim)
@@ -457,7 +456,11 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real}
         end
         @debug solution_summary(optim, verbose=true)
     end
-    mpc.ΔŨ .= iserror(optim) ? ΔŨlast : ΔŨcurr
+    if iserror(optim)
+        mpc.ΔŨ .= ΔŨ0
+    else
+        mpc.ΔŨ .= value.(ΔŨvar)
+    end
     return mpc.ΔŨ
 end
 
diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl
@@ -304,6 +304,7 @@ function init_optimization!(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where
     Jfunc, gfunc = let mpc=mpc, model=model, ng=ng, nΔŨ=nΔŨ, nŶ=Hp*ny, nx̂=nx̂, nu=nu, nU=Hp*nu
         Nc = nΔŨ + 3
         last_ΔŨtup_float, last_ΔŨtup_dual = nothing, nothing
+        ΔŨ_cache::DiffCache{Vector{JNT}, Vector{JNT}}    = DiffCache(zeros(JNT, nΔŨ), Nc)
         Ŷ_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nŶ), Nc)
         U_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nU), Nc)
         g_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, ng), Nc)
@@ -313,63 +314,42 @@ function init_optimization!(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where
         û_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nu), Nc)
         Ȳ_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nŶ), Nc)
         Ū_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nU), Nc)
-        function Jfunc(ΔŨtup::JNT...)
+        function Jfunc(ΔŨtup::T...)::T where {T <: Real}
             ΔŨ1 = ΔŨtup[begin]
-            Ŷ = get_tmp(Ŷ_cache, ΔŨ1)
-            ΔŨ = collect(ΔŨtup)
-            if ΔŨtup !== last_ΔŨtup_float
-                x̂, x̂next = get_tmp(x̂_cache, ΔŨ1), get_tmp(x̂next_cache, ΔŨ1)
-                u, û = get_tmp(u_cache, ΔŨ1), get_tmp(û_cache, ΔŨ1)
-                Ŷ, x̂end = predict!(Ŷ, x̂, x̂next, u, û, mpc, model, ΔŨ)
-                g = get_tmp(g_cache, ΔŨ1)
-                g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ)
+            if T == JNT
                 last_ΔŨtup_float = ΔŨtup
+            else
+                last_ΔŨtup_dual  = ΔŨtup
             end
+            ΔŨ, Ŷ = get_tmp(ΔŨ_cache, ΔŨ1), get_tmp(Ŷ_cache, ΔŨ1)
+            x̂, x̂next = get_tmp(x̂_cache, ΔŨ1), get_tmp(x̂next_cache, ΔŨ1)
+            u, û = get_tmp(u_cache, ΔŨ1), get_tmp(û_cache, ΔŨ1)
+            ΔŨ .= ΔŨtup
+            Ŷ, x̂end = predict!(Ŷ, x̂, x̂next, u, û, mpc, model, ΔŨ)
+            g = get_tmp(g_cache, ΔŨ1)
+            g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ)
             U, Ȳ, Ū = get_tmp(U_cache, ΔŨ1), get_tmp(Ȳ_cache, ΔŨ1), get_tmp(Ū_cache, ΔŨ1)
-            return obj_nonlinprog!(U, Ȳ, Ū, mpc, model, Ŷ, ΔŨ)
-        end
-        function Jfunc(ΔŨtup::ForwardDiff.Dual...)
-            ΔŨ1 = ΔŨtup[begin]
-            Ŷ = get_tmp(Ŷ_cache, ΔŨ1)
-            ΔŨ = collect(ΔŨtup)
-            if ΔŨtup !== last_ΔŨtup_dual
-                x̂, x̂next = get_tmp(x̂_cache, ΔŨ1), get_tmp(x̂next_cache, ΔŨ1)
-                u, û = get_tmp(u_cache, ΔŨ1), get_tmp(û_cache, ΔŨ1)
-                Ŷ, x̂end = predict!(Ŷ, x̂, x̂next, u, û, mpc, model, ΔŨ)
-                g = get_tmp(g_cache, ΔŨ1)
-                g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ)
-                last_ΔŨtup_dual = ΔŨtup
-            end
-            U, Ȳ, Ū = get_tmp(U_cache, ΔŨ1), get_tmp(Ȳ_cache, ΔŨ1), get_tmp(Ū_cache, ΔŨ1)
-            return obj_nonlinprog!(U, Ȳ, Ū, mpc, model, Ŷ, ΔŨ)
+            return obj_nonlinprog!(U, Ȳ, Ū, mpc, model, Ŷ, ΔŨ)::T
         end
-        function gfunc_i(i, ΔŨtup::NTuple{N, JNT}) where N
+        function gfunc_i(i, ΔŨtup::NTuple{N, T})::T where {N, T <:Real}
             ΔŨ1 = ΔŨtup[begin]
             g = get_tmp(g_cache, ΔŨ1)
-            if ΔŨtup !== last_ΔŨtup_float
-                Ŷ = get_tmp(Ŷ_cache, ΔŨ1)
-                ΔŨ = collect(ΔŨtup)
-                x̂, x̂next = get_tmp(x̂_cache, ΔŨ1), get_tmp(x̂next_cache, ΔŨ1)
-                u, û = get_tmp(u_cache, ΔŨ1), get_tmp(û_cache, ΔŨ1)
-                Ŷ, x̂end = predict!(Ŷ, x̂, x̂next, u, û, mpc, model, ΔŨ)
-                g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ)
-                last_ΔŨtup_float = ΔŨtup
+            if T == JNT
+                isnewvalue = (ΔŨtup !== last_ΔŨtup_float)
+                isnewvalue && (last_ΔŨtup_float = ΔŨtup)
+            else
+                isnewvalue = (ΔŨtup !== last_ΔŨtup_dual)
+                isnewvalue && (last_ΔŨtup_dual = ΔŨtup)
             end
-            return g[i]
-        end 
-        function gfunc_i(i, ΔŨtup::NTuple{N, ForwardDiff.Dual}) where N
-            ΔŨ1 = ΔŨtup[begin]
-            g = get_tmp(g_cache, ΔŨ1)
-            if ΔŨtup !== last_ΔŨtup_dual
-                Ŷ = get_tmp(Ŷ_cache, ΔŨ1)
-                ΔŨ = collect(ΔŨtup)
+            if isnewvalue
+                ΔŨ, Ŷ = get_tmp(ΔŨ_cache, ΔŨ1), get_tmp(Ŷ_cache, ΔŨ1)
                 x̂, x̂next = get_tmp(x̂_cache, ΔŨ1), get_tmp(x̂next_cache, ΔŨ1)
                 u, û = get_tmp(u_cache, ΔŨ1), get_tmp(û_cache, ΔŨ1)
+                ΔŨ .= ΔŨtup
                 Ŷ, x̂end = predict!(Ŷ, x̂, x̂next, u, û, mpc, model, ΔŨ)
                 g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ)
-                last_ΔŨtup_dual = ΔŨtup
             end
-            return g[i]
+            return g[i]::T
         end
         gfunc = [(ΔŨ...) -> gfunc_i(i, ΔŨ) for i in 1:ng]
         (Jfunc, gfunc)
diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl
@@ -1008,72 +1008,52 @@ function init_optimization!(
     Jfunc, gfunc = let estim=estim, model=model, nZ̃=nZ̃, nV̂=nV̂, nX̂=nX̂, ng=ng, nx̂=nx̂, nu=nu, nŷ=nŷ
         Nc = nZ̃ + 3
         last_Z̃tup_float, last_Z̃tup_dual = nothing, nothing
+        Z̃_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nZ̃), Nc)
         V̂_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nV̂), Nc)
         g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Nc)
         X̂_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nX̂), Nc)
         x̄_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Nc)
         û_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nu), Nc)
         ŷ_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŷ), Nc)
-        function Jfunc(Z̃tup::JNT...)
+        function Jfunc(Z̃tup::T...)::T where {T <: Real}
             Z̃1 = Z̃tup[begin]
-            V̂ = get_tmp(V̂_cache, Z̃1)
-            Z̃ = collect(Z̃tup)
-            if Z̃tup !== last_Z̃tup_float
-                g = get_tmp(g_cache, Z̃1)
-                X̂ = get_tmp(X̂_cache, Z̃1)
-                û, ŷ = get_tmp(û_cache, Z̃1), get_tmp(ŷ_cache, Z̃1)
-                V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, Z̃)
-                g = con_nonlinprog!(g, estim, model, X̂, V̂, Z̃)
+            if T == JNT
                 last_Z̃tup_float = Z̃tup
-            end
-            x̄ = get_tmp(x̄_cache, Z̃1)
-            return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)
-        end
-        function Jfunc(Z̃tup::ForwardDiff.Dual...)
-            Z̃1 = Z̃tup[begin]
-            V̂ = get_tmp(V̂_cache, Z̃1)
-            Z̃ = collect(Z̃tup)
-            if Z̃tup !== last_Z̃tup_dual
-                g = get_tmp(g_cache, Z̃1)
-                X̂ = get_tmp(X̂_cache, Z̃1)
-                û, ŷ = get_tmp(û_cache, Z̃1), get_tmp(ŷ_cache, Z̃1)
-                V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, Z̃)
-                g = con_nonlinprog!(g, estim, model, X̂, V̂, Z̃)
+            else
                 last_Z̃tup_dual = Z̃tup
             end
+            Z̃, V̂ = get_tmp(Z̃_cache, Z̃1), get_tmp(V̂_cache, Z̃1)
+            X̂ = get_tmp(X̂_cache, Z̃1)
+            û, ŷ = get_tmp(û_cache, Z̃1), get_tmp(ŷ_cache, Z̃1)
+            Z̃ .= Z̃tup
+            V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, Z̃)
+            g = get_tmp(g_cache, Z̃1)
+            g = con_nonlinprog!(g, estim, model, X̂, V̂, Z̃)
             x̄ = get_tmp(x̄_cache, Z̃1)
-            return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)
+            return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T
         end
-        function gfunc_i(i, Z̃tup::NTuple{N, JNT}) where N
+        function gfunc_i(i, Z̃tup::NTuple{N, T})::T where {N, T <:Real}
             Z̃1 = Z̃tup[begin]
             g = get_tmp(g_cache, Z̃1)
-            if Z̃tup !== last_Z̃tup_float
-                Z̃ = collect(Z̃tup)
-                V̂ = get_tmp(V̂_cache, Z̃1)
-                X̂ = get_tmp(X̂_cache, Z̃1)
-                û, ŷ = get_tmp(û_cache, Z̃1), get_tmp(ŷ_cache, Z̃1)
-                V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, Z̃)
-                g = con_nonlinprog!(g, estim, model, X̂, V̂, Z̃)
-                last_Z̃tup_float = Z̃tup
+            if T == JNT
+                isnewvalue = (Z̃tup !== last_Z̃tup_float)
+                isnewvalue && (last_Z̃tup_float = Z̃tup)
+            else
+                isnewvalue = (Z̃tup !== last_Z̃tup_dual)
+                isnewvalue && (last_Z̃tup_dual = Z̃tup)
             end
-            return g[i]
-        end 
-        function gfunc_i(i, Z̃tup::NTuple{N, ForwardDiff.Dual}) where N
-            Z̃1 = Z̃tup[begin]
-            g = get_tmp(g_cache, Z̃1)
-            if Z̃tup !== last_Z̃tup_dual
-                Z̃ = collect(Z̃tup)
-                V̂ = get_tmp(V̂_cache, Z̃1)
+            if isnewvalue
+                Z̃, V̂ = get_tmp(Z̃_cache, Z̃1), get_tmp(V̂_cache, Z̃1)
                 X̂ = get_tmp(X̂_cache, Z̃1)
                 û, ŷ = get_tmp(û_cache, Z̃1), get_tmp(ŷ_cache, Z̃1)
+                Z̃ .= Z̃tup
                 V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, Z̃)
                 g = con_nonlinprog!(g, estim, model, X̂, V̂, Z̃)
-                last_Z̃tup_dual = Z̃tup
             end
             return g[i]
         end
         gfunc = [(Z̃...) -> gfunc_i(i, Z̃) for i in 1:ng]
-        Jfunc, gfunc
+        (Jfunc, gfunc)
     end
     register(optim, :Jfunc, nZ̃, Jfunc, autodiff=true)
     @NLobjective(optim, Min, Jfunc(Z̃var...))
diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl
@@ -62,7 +62,6 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT<
         end
     end
     # -------- error handling -------------------------
-    Z̃curr, Z̃last = value.(Z̃var), Z̃0
     if !issolved(optim)
         status = termination_status(optim)
         if iserror(optim)
@@ -74,7 +73,11 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT<
         end
         @debug solution_summary(optim, verbose=true)
     end
-    estim.Z̃ .= iserror(optim) ? Z̃last : Z̃curr
+    if iserror(optim)
+        estim.Z̃ .= Z̃0
+    else
+        estim.Z̃ .= value.(Z̃var)
+    end
     # --------- update estimate -----------------------
     estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start
     V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, estim.Z̃)
diff --git a/src/general.jl b/src/general.jl
@@ -7,11 +7,11 @@ const DEFAULT_CWT = 1e5
 const DEFAULT_EWT = 0.0
 
 "Termination status that means 'no solution available'."
-const ERROR_STATUSES = [
+const ERROR_STATUSES = (
     INFEASIBLE, DUAL_INFEASIBLE, LOCALLY_INFEASIBLE, INFEASIBLE_OR_UNBOUNDED, 
     NUMERICAL_ERROR, INVALID_MODEL, INVALID_OPTION, INTERRUPTED, 
     OTHER_ERROR
-]
+)
 
 "Verify that `optim` termination status is `OPTIMAL` or `LOCALLY_SOLVED`."
 function issolved(optim::JuMP.GenericModel)
@@ -22,7 +22,7 @@ end
 "Verify that `optim` termination status means 'no solution available'."
 function iserror(optim::JuMP.GenericModel) 
     status = termination_status(optim)
-    return any(status .== ERROR_STATUSES)
+    return any(errstatus->isequal(status, errstatus), ERROR_STATUSES)
 end
 
 "Evaluate the quadratic programming objective function `0.5x'*H*x + q'*x` at `x`."
diff --git a/src/plot_sim.jl b/src/plot_sim.jl
@@ -129,7 +129,7 @@ function sim!(
         U_data[:, i]  = u
         D_data[:, i]  = d
         X_data[:, i]  = plant.x
-        x = updatestate!(plant, u, d)
+        updatestate!(plant, u, d)
     end
     return SimResult(plant, T_data, Y_data, U_data, Y_data, 
                      U_data, U_data, U_data, D_data, X_data, X_data)
diff --git a/test/test_state_estim.jl b/test/test_state_estim.jl
@@ -682,10 +682,12 @@ end
 
     mhe4 = setconstraint!(MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0), x̂max=[50,50,50,50])
     g_X̂max_end = mhe4.optim.nlp_model.operators.registered_multivariate_operators[end].f
-    @test g_X̂max_end((1.0, 1.0, 1.0, 1.0)) ≤ 0.0 # test gfunc_i(i,::NTuple{N, Float64})
-    # test gfunc_i(i,::NTuple{N, ForwardDiff.Dual}) : 
-    @test ForwardDiff.gradient(g_X̂max_end, [1.0, 1.0, 1.0, 1.0]) ≈ [0.0, 0.0, 0.0, 0.0]
-
+    # test gfunc_i(i,::NTuple{N, Float64}):
+    @test g_X̂max_end(
+        (1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0)) ≤ 0.0 
+    # test gfunc_i(i,::NTuple{N, ForwardDiff.Dual}): 
+    @test ForwardDiff.gradient(
+        g_X̂max_end, [1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0]) ≈ [0, 0, 0, 0, 0, 0, 0, 0]
     Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2) 
     R̂ = diagm([1, 1].^2)
     optim = Model(Ipopt.Optimizer)
