wip: idem

Author: franckgaga

src/controller/construct.jl

@@ -434,7 +434,7 @@ function setconstraint!(
         JuMP.delete(optim, optim[:linconstraint])
         JuMP.unregister(optim, :linconstraint)
         @constraint(optim, linconstraint, A*Z̃var .≤ b)
-        # TODO: change this
+        # TODO: change this !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
         if JuMP.solver_name(optim) ≠ "Ipopt"
             set_nonlincon!(mpc, model, transcription, optim)
         end

src/controller/nonlinmpc.jl

@@ -533,7 +533,7 @@ function init_optimization!(
         init_nonlincon!(mpc, model, transcription, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs!)
         set_nonlincon!(mpc, model, transcription, optim)
     else
-        # Test new experimental  feature:
+        # ========= Test new experimental  feature ========================================
 
         model = mpc.estim.model
         jac = mpc.jacobian
@@ -580,21 +580,23 @@ function init_optimization!(
                 Z̃_∇g .= Z̃_arg
                 value_and_jacobian!(gfunc!, g, ∇g, ∇g_prep, jac, Z̃_∇g, ∇g_context...)
             end
+            return nothing
         end
         function gfunc_set!(g_arg, Z̃_arg)
             update_con!(g, ∇g, Z̃_∇g, Z̃_arg)
-            return g_arg .= g
+            g_arg .= g
+            return nothing
         end
         function ∇gfunc_set!(∇g_arg, Z̃_arg)
             update_con!(g, ∇g, Z̃_∇g, Z̃_arg)
-            return ∇g_arg .= nonzeros(∇g)
+            diffmat2vec!(∇g_arg, ∇g)
+            return nothing
         end
 
         g_min = fill(-myInf, ng)
         g_max = fill(+myInf, ng)
 
-        I_∇g, J_∇g = SparseArrays.findnz(∇g)
-        ∇g_structure = collect(zip(I_∇g, J_∇g))
+        ∇g_structure = init_diffstructure(∇g)
 
         g_set = Ipopt._VectorNonlinearOracle(;
             dimension = nZ̃,
@@ -606,10 +608,6 @@ function init_optimization!(
         )
         @constraint(optim, Z̃var in g_set)
 
-
-
-
-
         function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g) 
             update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃)
             return nothing
@@ -629,21 +627,23 @@ function init_optimization!(
                 Z̃_∇geq .= Z̃_arg
                 value_and_jacobian!(geqfunc!, geq, ∇geq, ∇geq_prep, jac, Z̃_∇geq, ∇geq_context...)
             end
+            return nothing
         end
         function geqfunc_set!(geq_arg, Z̃_arg)
             update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃_arg)
-            return geq_arg .= geq
+            geq_arg .= geq
+            return nothing
         end
         function ∇geqfunc_set!(∇geq_arg, Z̃_arg)
             update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃_arg)
-            return ∇geq_arg .= nonzeros(∇geq)
+            diffmat2vec!(∇geq_arg, ∇geq)
+            return nothing
         end
 
         geq_min = zeros(JNT, mpc.con.neq)
         geq_max = zeros(JNT, mpc.con.neq)
 
-        I_∇geq, J_∇geq = SparseArrays.findnz(∇geq)
-        ∇geq_structure = collect(zip(I_∇geq, J_∇geq))
+        ∇geq_structure = init_diffstructure(∇geq)
 
         #=
         # Langragian of the optimization problem:

src/general.jl

@@ -58,8 +58,22 @@ function limit_solve_time(optim::GenericModel, Ts)
 end
 
 "Init a differentiation result matrix as dense or sparse matrix, as required by `backend`."
-init_diffmat(T, ::AbstractADType, _  , nx , ny) = Matrix{T}(undef, ny, nx)
-init_diffmat(T, ::AutoSparse    ,prep , _ , _ ) = similar(sparsity_pattern(prep), T)
+init_diffmat(T, ::AbstractADType, _ , nx, ny) = zeros(T, ny, nx)
+function init_diffmat(T, ::AutoSparse, prep , _ , _ )
+    A = similar(sparsity_pattern(prep), T)
+    return A .= 0
+end
+
+"Init the sparsity structure of matrix `A` as required by `JuMP.jl`."
+function init_diffstructure(A::AbstractSparseMatrix)
+    I, J = findnz(A)
+    return collect(zip(I, J))
+end
+init_diffstructure(A::AbstractMatrix)= Tuple.(CartesianIndices(A))[:]
+
+"Store the differentiation matrix `A` in the vector `v` as required by `JuMP.jl.`" 
+diffmat2vec!(v::AbstractVector, A::AbstractSparseMatrix)    = v   .= nonzeros(A)
+diffmat2vec!(v::AbstractVector, A::AbstractMatrix)          = v[:] = A
 
 "Verify that x and y elements are different using `!==`."
 isdifferent(x, y) = any(xi !== yi for (xi, yi) in zip(x, y))