changed: trunk g vector earlier

This is more efficient with dense differentiation, in theory. I will compare the perf. in a separate PR.

Author: franckgaga

src/controller/nonlinmpc.jl

@@ -737,7 +737,8 @@ function get_nonlinops(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where JNT<
     nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ
     nk = get_nk(model, transcription)
     Hp, Hc = mpc.Hp, mpc.Hc
-    ng, nc, neq = length(mpc.con.i_g), mpc.con.nc, mpc.con.neq
+    ng, ng_i_g = length(mpc.con.i_g), sum(mpc.con.i_g)
+    nc, neq = mpc.con.nc, mpc.con.neq
     nZ̃, nU, nŶ, nX̂, nK = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nk
     nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny  
     strict = Val(true)
@@ -750,45 +751,40 @@ function get_nonlinops(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where JNT<
     U0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU),  zeros(JNT, nŶ)
     Û0::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nU),  zeros(JNT, nX̂)
     gc::Vector{JNT}, g::Vector{JNT}  = zeros(JNT, nc),  zeros(JNT, ng)
+    g_i_g::Vector{JNT}               = zeros(JNT, ng_i_g)
     geq::Vector{JNT}                 = zeros(JNT, neq)
     # -------------- inequality constraint: nonlinear oracle -----------------------------
-    function g!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq)
+    function g_i_g!(g_i_g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g)
         update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃)
+        g_i_g .= @views g[mpc.con.i_g]
         return nothing
     end
     Z̃_∇g = fill(myNaN, nZ̃)      # NaN to force update_predictions! at first call
     ∇g_context = (
         Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), 
         Cache(Û0), Cache(K0), Cache(X̂0), 
-        Cache(gc), Cache(geq),
+        Cache(gc), Cache(geq), Cache(g)
     )
-    ## temporarily enable all the inequality constraints for sparsity detection:
-    # mpc.con.i_g[1:end-nc] .= true
-    ∇g_prep  = prepare_jacobian(g!, g, jac, Z̃_∇g, ∇g_context...; strict)
-    # mpc.con.i_g[1:end-nc] .= false
-    ∇g = init_diffmat(JNT, jac, ∇g_prep, nZ̃, ng)
-    function update_con!(g, ∇g, Z̃_∇g, Z̃_arg)
+    ∇g_prep = prepare_jacobian(g_i_g!, g_i_g, jac, Z̃_∇g, ∇g_context...; strict)
+    ∇g_i_g  = init_diffmat(JNT, jac, ∇g_prep, nZ̃, ng)
+    function update_con!(g_i_g, ∇g_i_g, Z̃_∇g, Z̃_arg)
         if isdifferent(Z̃_arg, Z̃_∇g)
             Z̃_∇g .= Z̃_arg
-            value_and_jacobian!(g!, g, ∇g, ∇g_prep, jac, Z̃_∇g, ∇g_context...)
+            value_and_jacobian!(g_i_g!, g_i_g, ∇g_i_g, ∇g_prep, jac, Z̃_∇g, ∇g_context...)
         end
         return nothing
     end
-    function gfunc_oracle!(g_arg, Z̃_arg)
-        update_con!(g, ∇g, Z̃_∇g, Z̃_arg)
-        g_arg .= @views g[mpc.con.i_g]
-        return nothing
+    function gfunc_oracle!(g_vec, Z̃_arg)
+        update_con!(g_i_g, ∇g_i_g, Z̃_∇g, Z̃_arg)
+        return g_vec .= g_i_g
     end
-    ∇g_i_g = ∇g[mpc.con.i_g, :]
-    function ∇gfunc_oracle!(∇g_arg, Z̃_arg)
-        update_con!(g, ∇g, Z̃_∇g, Z̃_arg)
-        ∇g_i_g .= @views ∇g[mpc.con.i_g, :]
-        diffmat2vec!(∇g_arg, ∇g_i_g)
-        return nothing
+    function ∇gfunc_oracle!(∇g_vec, Z̃_arg)
+        update_con!(g_i_g, ∇g_i_g, Z̃_∇g, Z̃_arg)
+        return diffmat2vec!(∇g_vec, ∇g_i_g)
     end
-    g_min = fill(-myInf, sum(mpc.con.i_g))
-    g_max = zeros(JNT, sum(mpc.con.i_g))
-    ∇g_structure = init_diffstructure(∇g[mpc.con.i_g, :])
+    g_min = fill(-myInf, ng_i_g)
+    g_max = zeros(JNT,   ng_i_g)
+    ∇g_structure = init_diffstructure(∇g_i_g)
     g_oracle = Ipopt._VectorNonlinearOracle(;
         dimension = nZ̃,
         l = g_min,
@@ -817,15 +813,13 @@ function get_nonlinops(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where JNT<
         end
         return nothing
     end
-    function geq_oracle!(geq_arg, Z̃_arg)
+    function geq_oracle!(geq_vec, Z̃_arg)
         update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃_arg)
-        geq_arg .= geq
-        return nothing
+        return geq_vec .= geq
     end
-    function ∇geq_oracle!(∇geq_arg, Z̃_arg)
+    function ∇geq_oracle!(∇geq_vec, Z̃_arg)
         update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃_arg)
-        diffmat2vec!(∇geq_arg, ∇geq)
-        return nothing
+        return diffmat2vec!(∇geq_vec, ∇geq)
     end
     geq_min = geq_max = zeros(JNT, neq)
     ∇geq_structure = init_diffstructure(∇geq)