From 82b70f90a3f158c9453e23a533fbf55e820bff53 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 16 Oct 2025 17:23:05 -0400 Subject: [PATCH 1/3] changed: trunk `g` vector earlier This is more efficient with dense differentiation, in theory. I will compare the perf. in a separate PR. --- src/controller/nonlinmpc.jl | 52 ++++++++++++++++--------------------- 1 file changed, 23 insertions(+), 29 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 6057dc8bb..1082127cb 100644 --- a/src/controller/nonlinmpc.jl +++ b/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) From 92684036df964cc56b74ef149a686caf4210ea5f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 18 Oct 2025 15:58:12 -0400 Subject: [PATCH 2/3] debug: do not call `@constraint` when no nonlinear constraints --- src/controller/nonlinmpc.jl | 86 ++++++++++++++++++------------------- 1 file changed, 43 insertions(+), 43 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 1082127cb..012cda764 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -729,7 +729,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT end # TODO: move docstring of method above here an re-work it -function get_nonlinops(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where JNT<:Real +function get_nonlinops(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real # ----------- common cache for all functions ---------------------------------------- model = mpc.estim.model transcription = mpc.transcription @@ -737,61 +737,61 @@ 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, ng_i_g = length(mpc.con.i_g), sum(mpc.con.i_g) + i_g = findall(mpc.con.i_g) # convert to non-logical indices for non-allocating @views + ng, ngi = 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) - myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) - J::Vector{JNT} = zeros(JNT, 1) - ΔŨ::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) - g_i_g::Vector{JNT} = zeros(JNT, ng_i_g) - geq::Vector{JNT} = zeros(JNT, neq) + myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) + J::Vector{JNT} = zeros(JNT, 1) + ΔŨ::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) + gi::Vector{JNT}, geq::Vector{JNT} = zeros(JNT, ngi), zeros(JNT, neq) # -------------- inequality constraint: nonlinear oracle ----------------------------- - function g_i_g!(g_i_g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g) + function gi!(gi, 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] + gi .= @views g[i_g] return nothing end - Z̃_∇g = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call - ∇g_context = ( + Z̃_∇gi = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call + ∇gi_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(g) ) - ∇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_i_g!, g_i_g, ∇g_i_g, ∇g_prep, jac, Z̃_∇g, ∇g_context...) + ∇gi_prep = prepare_jacobian(gi!, gi, jac, Z̃_∇gi, ∇gi_context...; strict) + ∇gi = init_diffmat(JNT, jac, ∇gi_prep, nZ̃, ng) + function update_con!(gi, ∇gi, Z̃_∇gi, Z̃_arg) + if isdifferent(Z̃_arg, Z̃_∇gi) + Z̃_∇gi .= Z̃_arg + value_and_jacobian!(gi!, gi, ∇gi, ∇gi_prep, jac, Z̃_∇gi, ∇gi_context...) end return nothing end - 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 + function gi_func!(gi_vec, Z̃_arg) + update_con!(gi, ∇gi, Z̃_∇gi, Z̃_arg) + return gi_vec .= gi end - 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) + function ∇gi_func!(∇gi_vec, Z̃_arg) + update_con!(gi, ∇gi, Z̃_∇gi, Z̃_arg) + return diffmat2vec!(∇gi_vec, ∇gi) end - g_min = fill(-myInf, ng_i_g) - g_max = zeros(JNT, ng_i_g) - ∇g_structure = init_diffstructure(∇g_i_g) + gi_min = fill(-myInf, ngi) + gi_max = zeros(JNT, ngi) + ∇gi_structure = init_diffstructure(∇gi) g_oracle = Ipopt._VectorNonlinearOracle(; dimension = nZ̃, - l = g_min, - u = g_max, - eval_f = gfunc_oracle!, - jacobian_structure = ∇g_structure, - eval_jacobian = ∇gfunc_oracle! + l = gi_min, + u = gi_max, + eval_f = gi_func!, + jacobian_structure = ∇gi_structure, + eval_jacobian = ∇gi_func! ) # ------------- equality constraints : nonlinear oracle ------------------------------ function geq!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g) @@ -813,11 +813,11 @@ function get_nonlinops(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where JNT< end return nothing end - function geq_oracle!(geq_vec, Z̃_arg) + function geq_func!(geq_vec, Z̃_arg) update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃_arg) return geq_vec .= geq end - function ∇geq_oracle!(∇geq_vec, Z̃_arg) + function ∇geq_func!(∇geq_vec, Z̃_arg) update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃_arg) return diffmat2vec!(∇geq_vec, ∇geq) end @@ -827,9 +827,9 @@ function get_nonlinops(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where JNT< dimension = nZ̃, l = geq_min, u = geq_max, - eval_f = geq_oracle!, + eval_f = geq_func!, jacobian_structure = ∇geq_structure, - eval_jacobian = ∇geq_oracle! + eval_jacobian = ∇geq_func! ) # ------------- objective function: splatting syntax --------------------------------- function J!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq) @@ -903,8 +903,8 @@ function set_nonlincon_exp!( optim, JuMP.Vector{JuMP.VariableRef}, Ipopt._VectorNonlinearOracle ) map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints) - @constraint(optim, Z̃var in g_oracle) - mpc.con.neq > 0 && @constraint(optim, Z̃var in geq_oracle) + any(mpc.con.i_g) && @constraint(optim, Z̃var in g_oracle) + mpc.con.neq > 0 && @constraint(optim, Z̃var in geq_oracle) return nothing end From d2441944017e83effddcfb99bbca2878ffe25b66 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 19 Oct 2025 13:04:20 -0400 Subject: [PATCH 3/3] debug: jacobian matrix with `ngi` lines --- src/controller/nonlinmpc.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 012cda764..ef1161ea6 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -766,7 +766,7 @@ function get_nonlinops(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real Cache(gc), Cache(geq), Cache(g) ) ∇gi_prep = prepare_jacobian(gi!, gi, jac, Z̃_∇gi, ∇gi_context...; strict) - ∇gi = init_diffmat(JNT, jac, ∇gi_prep, nZ̃, ng) + ∇gi = init_diffmat(JNT, jac, ∇gi_prep, nZ̃, ngi) function update_con!(gi, ∇gi, Z̃_∇gi, Z̃_arg) if isdifferent(Z̃_arg, Z̃_∇gi) Z̃_∇gi .= Z̃_arg