diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 711cb3040..15c64259e 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -434,7 +434,12 @@ function setconstraint!( JuMP.delete(optim, optim[:linconstraint]) JuMP.unregister(optim, :linconstraint) @constraint(optim, linconstraint, A*Z̃var .≤ b) - set_nonlincon!(mpc, model, transcription, optim) + # TODO: change this !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + if JuMP.solver_name(optim) ≠ "Ipopt" + set_nonlincon!(mpc, model, transcription, optim) + else + set_nonlincon_exp(mpc, optim) + end else i_b, i_g = init_matconstraint_mpc( model, transcription, nc, diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 5b5d2050a..911c728cc 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -498,7 +498,9 @@ end Init the nonlinear optimization for [`NonLinMPC`](@ref) controllers. """ -function init_optimization!(mpc::NonLinMPC, model::SimModel, optim::JuMP.GenericModel) +function init_optimization!( + mpc::NonLinMPC, model::SimModel, optim::JuMP.GenericModel{JNT} +) where JNT<:Real # --- variables and linear constraints --- con, transcription = mpc.con, mpc.transcription nZ̃ = length(mpc.Z̃) @@ -527,8 +529,162 @@ function init_optimization!(mpc::NonLinMPC, model::SimModel, optim::JuMP.Generic ) @operator(optim, J, nZ̃, Jfunc, ∇Jfunc!) @objective(optim, Min, J(Z̃var...)) - init_nonlincon!(mpc, model, transcription, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs!) - set_nonlincon!(mpc, model, transcription, optim) + if JuMP.solver_name(optim) ≠ "Ipopt" + init_nonlincon!(mpc, model, transcription, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs!) + set_nonlincon!(mpc, model, transcription, optim) + else + set_nonlincon_exp(mpc, optim) + end + return nothing +end + +set_nonlincon_exp(::PredictiveController, ::JuMP.GenericModel) = nothing +# TODO: cleanup this function, this is super dirty +function set_nonlincon_exp(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where JNT<:Real + # ========= Test new experimental feature ======================================== + + nonlin_constraints = JuMP.all_constraints(optim, JuMP.Vector{JuMP.VariableRef}, Ipopt._VectorNonlinearOracle) + map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints) + + + Z̃var = optim[:Z̃var] + + model = mpc.estim.model + jac = mpc.jacobian + nu, ny, nx̂, nϵ, nk = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, model.nk + Hp, Hc = mpc.Hp, mpc.Hc + ng, nc, neq = length(mpc.con.i_g), 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) + myNaN = convert(JNT, NaN) + myInf = convert(JNT, Inf) + + + ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) + x̂0end::Vector{JNT} = zeros(JNT, nx̂) + K0::Vector{JNT} = zeros(JNT, nK) + Ue::Vector{JNT}, Ŷe::Vector{JNT} = zeros(JNT, nUe), zeros(JNT, nŶe) + 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) + geq::Vector{JNT} = zeros(JNT, neq) + + + + # TODO: transfer all the following in set_nonlincon!, including a copy-paste + # of all the vectors above. + function gfunc!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + 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), + ) + # temporarily enable all the inequality constraints for sparsity detection: + mpc.con.i_g[1:end-nc] .= true + ∇g_prep = prepare_jacobian(gfunc!, 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) + if isdifferent(Z̃_arg, Z̃_∇g) + 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) + g_arg .= @views g[mpc.con.i_g] + return nothing + end + ∇g_i_g = ∇g[mpc.con.i_g, :] + function ∇gfunc_set!(∇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 + 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_set = Ipopt._VectorNonlinearOracle(; + dimension = nZ̃, + l = g_min, + u = g_max, + eval_f = gfunc_set!, + jacobian_structure = ∇g_structure, + eval_jacobian = ∇gfunc_set! + ) + @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 + end + Z̃_∇geq = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call + ∇geq_context = ( + Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), + Cache(Û0), Cache(K0), Cache(X̂0), + Cache(gc), Cache(g) + ) + ∇geq_prep = prepare_jacobian(geqfunc!, geq, jac, Z̃_∇geq, ∇geq_context...; strict) + ∇geq = init_diffmat(JNT, jac, ∇geq_prep, nZ̃, neq) + + + function update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃_arg) + if isdifferent(Z̃_arg, Z̃_∇geq) + 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) + geq_arg .= geq + return nothing + end + function ∇geqfunc_set!(∇geq_arg, Z̃_arg) + update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃_arg) + diffmat2vec!(∇geq_arg, ∇geq) + return nothing + end + + geq_min = zeros(JNT, mpc.con.neq) + geq_max = zeros(JNT, mpc.con.neq) + + ∇geq_structure = init_diffstructure(∇geq) + + #= + # Langragian of the optimization problem: + function Lfunc!(Z̃, μ, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + J = obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) + L = J + dot(μ, geq) + return L + end + =# + + + geq_set = Ipopt._VectorNonlinearOracle(; + dimension = nZ̃, + l = geq_min, + u = geq_max, + eval_f = geqfunc_set!, + jacobian_structure = ∇geq_structure, + eval_jacobian = ∇geqfunc_set! + ) + @constraint(optim, Z̃var in geq_set) return nothing end diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index ba9f92103..0ed7b6860 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1173,7 +1173,7 @@ 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) + nŶ = length(Ŷ0) for i in eachindex(g) mpc.con.i_g[i] || continue if i ≤ nŶ diff --git a/src/general.jl b/src/general.jl index f5e8ecb95..0cd4849da 100644 --- a/src/general.jl +++ b/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, backend::AbstractADType, _ , nx , ny) = Matrix{T}(undef, ny, nx) -init_diffmat(T, backend::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))