From cd98810632ca0165378a7209c97db61d7f951139 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Mar 2025 17:37:26 -0400 Subject: [PATCH 01/30] added: `NonLinMPC` and `MovingHorizonEstimator` integration with DI.jl --- Project.toml | 27 ++++------ README.md | 14 +++-- docs/src/index.md | 4 +- src/ModelPredictiveControl.jl | 12 ++++- src/controller/linmpc.jl | 4 +- src/controller/nonlinmpc.jl | 94 +++++++++++++++++++++++----------- src/estimator/mhe/construct.jl | 81 ++++++++++++++++++++--------- src/general.jl | 16 +----- src/model/linearization.jl | 10 ++-- 9 files changed, 166 insertions(+), 96 deletions(-) diff --git a/Project.toml b/Project.toml index 3aeb327cc..6c3efad23 100644 --- a/Project.toml +++ b/Project.toml @@ -4,48 +4,43 @@ authors = ["Francis Gagnon"] version = "1.4.2" [deps] -LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" -Logging = "56ddb016-857b-54e1-b83d-db4d58db5568" -Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" +DifferentiationInterface = "a0c0ee7d-e4b9-4e03-894e-1c5f64a51d63" ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" JuMP = "4076af6c-e467-56ae-b986-b466b2749572" +LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +Logging = "56ddb016-857b-54e1-b83d-db4d58db5568" OSQP = "ab2f91bb-94b4-55e3-9ba0-7f65df51de79" PreallocationTools = "d236fae5-4411-538c-8e31-a6e3d9e00b46" PrecompileTools = "aea7be01-6a6a-4083-8856-8a6e6704d82a" ProgressLogging = "33c8b6b6-d38a-422a-b730-caa89a2f386c" +Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" RecipesBase = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" [compat] -julia = "1.10" -LinearAlgebra = "1.10" -Logging = "1.10" -Random = "1.10" ControlSystemsBase = "1.9" +DifferentiationInterface = "0.6.43" ForwardDiff = "0.10" Ipopt = "1" JuMP = "1.21" +LinearAlgebra = "1.10" +Logging = "1.10" OSQP = "0.8" PreallocationTools = "0.4.14" PrecompileTools = "1" ProgressLogging = "0.1" +Random = "1.10" RecipesBase = "1" +julia = "1.10" [extras] DAQP = "c47d62df-3981-49c8-9651-128b1cd08617" Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4" Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" -TestItems = "1c621080-faea-4a02-84b6-bbd5e436b8fe" TestItemRunner = "f8b46487-2199-4994-9208-9a1283c18c0a" +TestItems = "1c621080-faea-4a02-84b6-bbd5e436b8fe" [targets] -test = [ - "Test", - "TestItems", - "TestItemRunner", - "Documenter", - "Plots", - "DAQP" - ] +test = ["Test", "TestItems", "TestItemRunner", "Documenter", "Plots", "DAQP"] diff --git a/README.md b/README.md index c93120ee4..63bb8bfc0 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,9 @@ An open source [model predictive control](https://en.wikipedia.org/wiki/Model_pr package for Julia. The package depends on [`ControlSystemsBase.jl`](https://github.com/JuliaControl/ControlSystems.jl) -for the linear systems and [`JuMP.jl`](https://github.com/jump-dev/JuMP.jl) for the solving. +for the linear systems, [`JuMP.jl`](https://github.com/jump-dev/JuMP.jl) for the +optimization and [`DifferentiationInterface.jl`](https://github.com/JuliaDiff/DifferentiationInterface.jl) +for the differentiation. ## Installation @@ -102,9 +104,13 @@ for more detailed examples. - measured disturbances - input setpoints - easy integration with `Plots.jl` -- optimization based on `JuMP.jl`: - - quickly compare multiple optimizers - - nonlinear solvers relying on automatic differentiation (exact derivative) +- optimization based on `JuMP.jl` to quickly compare multiple optimizers: + - many quadratic solvers for linear control + - many nonlinear solvers for nonlinear control (local or global) +- derivatives based on `DifferentiationInterface.jl` to compare different approaches: + - automatic differentiation (exact solution) + - symbolic differentiation (exact solution) + - finite difference (approximate solution) - supported transcription methods of the optimization problem: - direct single shooting - direct multiple shooting diff --git a/docs/src/index.md b/docs/src/index.md index 230b4459e..278fb1efd 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -4,7 +4,9 @@ An open source [model predictive control](https://en.wikipedia.org/wiki/Model_pr package for Julia. The package depends on [`ControlSystemsBase.jl`](https://github.com/JuliaControl/ControlSystems.jl) -for the linear systems and [`JuMP.jl`](https://github.com/jump-dev/JuMP.jl) for the solving. +for the linear systems, [`JuMP.jl`](https://github.com/jump-dev/JuMP.jl) for the +optimization and [`DifferentiationInterface.jl`](https://github.com/JuliaDiff/DifferentiationInterface.jl) +for the differentiation. The objective is to provide a simple, clear and modular framework to quickly design model predictive controllers (MPCs) in Julia, while preserving the flexibility for advanced diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index 95d8acf25..fdcd90b33 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -6,7 +6,17 @@ using Random: randn using RecipesBase using ProgressLogging -using ForwardDiff + + + +using DifferentiationInterface: ADTypes.AbstractADType, AutoForwardDiff, AutoSparse +using DifferentiationInterface: gradient!, jacobian!, prepare_gradient, prepare_jacobian + +import ForwardDiff + + + + import ControlSystemsBase import ControlSystemsBase: ss, tf, delay diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 224d0fdf7..5ce357404 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -259,11 +259,11 @@ function LinMPC( end """ - init_optimization!(mpc::LinMPC, model::LinModel, optim) + init_optimization!(mpc::LinMPC, model::LinModel, optim::JuMP.GenericModel) -> nothing Init the quadratic optimization for [`LinMPC`](@ref) controllers. """ -function init_optimization!(mpc::LinMPC, model::LinModel, optim) +function init_optimization!(mpc::LinMPC, model::LinModel, optim::JuMP.GenericModel) # --- variables and linear constraints --- con = mpc.con nZ̃ = length(mpc.Z̃) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 269925d8e..1b85a23ad 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -1,5 +1,6 @@ -const DEFAULT_NONLINMPC_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes") const DEFAULT_NONLINMPC_TRANSCRIPTION = SingleShooting() +const DEFAULT_NONLINMPC_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes") +const DEFAULT_NONLINMPC_GRADIENT = AutoForwardDiff() struct NonLinMPC{ NT<:Real, @@ -52,7 +53,7 @@ struct NonLinMPC{ function NonLinMPC{NT}( estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gc!::GCfunc, nc, p::PT, - transcription::TM, optim::JM + transcription::TM, optim::JM, gradient, jacobian ) where { NT<:Real, SE<:StateEstimator, @@ -109,7 +110,7 @@ struct NonLinMPC{ Uop, Yop, Dop, buffer ) - init_optimization!(mpc, model, optim) + init_optimization!(mpc, model, optim, gradient, jacobian) return mpc end end @@ -189,6 +190,10 @@ This controller allocates memory at each time step for the optimization. - `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the predictive controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model) (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl) optimizer). +- `gradient=AutoForwardDiff()` : an `AbstractADType` backend for the gradient of the objective + function, see [`DifferentiationInterface`](https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/explanation/backends/#List). +- `jacobian=default_jacobian(transcription)` : an `AbstractADType` backend for the Jacobian + of the nonlinear constraints, see `gradient` above for the options (default in Extended Help). - additional keyword arguments are passed to [`UnscentedKalmanFilter`](@ref) constructor (or [`SteadyKalmanFilter`](@ref), for [`LinModel`](@ref)). @@ -265,13 +270,15 @@ function NonLinMPC( p = model.p, transcription::TranscriptionMethod = DEFAULT_NONLINMPC_TRANSCRIPTION, optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), + gradient::AbstractADType = DEFAULT_NONLINMPC_GRADIENT, + jacobian::AbstractADType = default_jacobian(transcription), kwargs... ) estim = UnscentedKalmanFilter(model; kwargs...) return NonLinMPC( estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gc, nc, p, M_Hp, N_Hc, L_Hp, - transcription, optim + transcription, optim, gradient, jacobian ) end @@ -294,13 +301,15 @@ function NonLinMPC( p = model.p, transcription::TranscriptionMethod = DEFAULT_NONLINMPC_TRANSCRIPTION, optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), + gradient::AbstractADType = DEFAULT_NONLINMPC_GRADIENT, + jacobian::AbstractADType = default_jacobian(transcription), kwargs... ) estim = SteadyKalmanFilter(model; kwargs...) return NonLinMPC( estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gc, nc, p, M_Hp, N_Hc, L_Hp, - transcription, optim + transcription, optim, gradient, jacobian ) end @@ -347,6 +356,8 @@ function NonLinMPC( p = estim.model.p, transcription::TranscriptionMethod = DEFAULT_NONLINMPC_TRANSCRIPTION, optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), + gradient::AbstractADType = DEFAULT_NONLINMPC_GRADIENT, + jacobian::AbstractADType = default_jacobian(transcription), ) where { NT<:Real, SE<:StateEstimator{NT} @@ -360,10 +371,13 @@ function NonLinMPC( gc! = get_mutating_gc(NT, gc) return NonLinMPC{NT}( estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, gc!, nc, p, - transcription, optim + transcription, optim, gradient, jacobian ) end +default_jacobian(::SingleShooting) = AutoForwardDiff() +default_jacobian(::TranscriptionMethod) = AutoForwardDiff() + """ validate_JE(NT, JE) -> nothing @@ -477,11 +491,23 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real end """ - init_optimization!(mpc::NonLinMPC, model::SimModel, optim) + init_optimization!( + mpc::NonLinMPC, + model::SimModel, + optim::JuMP.GenericModel, + gradient::AbstractADType, + Jacobian::AbstractADType + ) -> nothing Init the nonlinear optimization for [`NonLinMPC`](@ref) controllers. """ -function init_optimization!(mpc::NonLinMPC, model::SimModel, optim) +function init_optimization!( + mpc::NonLinMPC, + model::SimModel, + optim::JuMP.GenericModel, + gradient::AbstractADType, + jacobian::AbstractADType + ) # --- variables and linear constraints --- con, transcription = mpc.con, mpc.transcription nZ̃ = length(mpc.Z̃) @@ -505,7 +531,9 @@ function init_optimization!(mpc::NonLinMPC, model::SimModel, optim) JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) end end - Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! = get_optim_functions(mpc, optim) + Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! = get_optim_functions( + mpc, optim, gradient, jacobian + ) @operator(optim, J, nZ̃, Jfunc, ∇Jfunc!) @objective(optim, Min, J(Z̃var...)) init_nonlincon!(mpc, model, transcription, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs!) @@ -515,7 +543,10 @@ end """ get_optim_functions( - mpc::NonLinMPC, optim::JuMP.GenericModel + mpc::NonLinMPC, + optim::JuMP.GenericModel, + grad_backend::AbstractADType, + jac_backend ::AbstractADType ) -> Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! Return the functions for the nonlinear optimization of `mpc` [`NonLinMPC`](@ref) controller. @@ -538,7 +569,12 @@ This method is really intricate and I'm not proud of it. That's because of 3 ele Inspired from: [User-defined operators with vector outputs](https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs) """ -function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real +function get_optim_functions( + mpc::NonLinMPC, + optim::JuMP.GenericModel{JNT}, + grad_backend::AbstractADType, + jac_backend ::AbstractADType +) where JNT<:Real model, transcription = mpc.estim.model, mpc.transcription nu, ny, nx̂, nϵ, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, mpc.Hp, mpc.Hc ng, nc, neq = length(mpc.con.i_g), mpc.con.nc, mpc.con.neq @@ -602,19 +638,19 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT U0, Ŷ0 = get_tmp(U0_cache, T), get_tmp(Ŷ0_cache, T) return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T end - Z̃_∇J = fill(myNaN, nZ̃) - ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J - ∇J_buffer = GradientBuffer(Jfunc_vec, Z̃_∇J) + Z̃_∇J = fill(myNaN, nZ̃) + ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J + ∇J_prep = prepare_gradient(Jfunc_vec, grad_backend, Z̃_∇J) ∇Jfunc! = if nZ̃ == 1 function (Z̃arg::T) where T<:Real Z̃_∇J .= Z̃arg - gradient!(∇J, ∇J_buffer, Z̃_∇J) + gradient!(Jfunc_vec, ∇J, ∇J_prep, grad_backend, Z̃_∇J) return ∇J[begin] # univariate syntax, see JuMP.@operator doc end else function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} Z̃_∇J .= Z̃arg - gradient!(∇J, ∇J_buffer, Z̃_∇J) + gradient!(Jfunc_vec, ∇J, ∇J_prep, grad_backend, Z̃_∇J) return ∇J # multivariate syntax, see JuMP.@operator doc end end @@ -633,17 +669,17 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT g .= get_tmp(g_cache, T) return g end - Z̃_∇g = fill(myNaN, nZ̃) - g_vec = Vector{JNT}(undef, ng) - ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of inequality constraints g - ∇g_buffer = JacobianBuffer(gfunc_vec!, g_vec, Z̃_∇g) - ∇gfuncs! = Vector{Function}(undef, ng) + Z̃_∇g = fill(myNaN, nZ̃) + g_vec = Vector{JNT}(undef, ng) + ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of inequality constraints g + ∇g_prep = prepare_jacobian(gfunc_vec!, g_vec, jac_backend, Z̃_∇g) + ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) ∇gfuncs![i] = if nZ̃ == 1 function (Z̃arg::T) where T<:Real if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!(∇g, ∇g_buffer, g_vec, Z̃_∇g) + jacobian!(gfunc_vec!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g) end return ∇g[i, begin] # univariate syntax, see JuMP.@operator doc end @@ -651,7 +687,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!(∇g, ∇g_buffer, g_vec, Z̃_∇g) + jacobian!(gfunc_vec!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g) end return ∇g_i .= @views ∇g[i, :] # multivariate syntax, see JuMP.@operator doc end @@ -672,11 +708,11 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT geq .= get_tmp(geq_cache, T) return geq end - Z̃_∇geq = fill(myNaN, nZ̃) # NaN to force update at 1st call - geq_vec = Vector{JNT}(undef, neq) - ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of equality constraints geq - ∇geq_buffer = JacobianBuffer(geqfunc_vec!, geq_vec, Z̃_∇geq) - ∇geqfuncs! = Vector{Function}(undef, neq) + Z̃_∇geq = fill(myNaN, nZ̃) + geq_vec = Vector{JNT}(undef, neq) + ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of equality constraints geq + ∇geq_prep = prepare_jacobian(geqfunc_vec!, geq_vec, jac_backend, Z̃_∇geq) + ∇geqfuncs! = Vector{Function}(undef, neq) for i in eachindex(∇geqfuncs!) # only multivariate syntax, univariate is impossible since nonlinear equality # constraints imply MultipleShooting, thus input increment ΔU and state X̂0 in Z̃: @@ -684,7 +720,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT function (∇geq_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃_∇geq) Z̃_∇geq .= Z̃arg - jacobian!(∇geq, ∇geq_buffer, geq_vec, Z̃_∇geq) + jacobian!(geqfunc_vec!, geq_vec, ∇geq, ∇geq_prep, jac_backend, Z̃_∇geq) end return ∇geq_i .= @views ∇geq[i, :] end diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 953181384..90f818fa6 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1,5 +1,7 @@ const DEFAULT_LINMHE_OPTIMIZER = OSQP.MathOptInterfaceOSQP.Optimizer const DEFAULT_NONLINMHE_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes") +const DEFAULT_NONLINMHE_GRADIENT = AutoForwardDiff() +const DEFAULT_NONLINMHE_JACOBIAN = AutoForwardDiff() @doc raw""" Include all the data for the constraints of [`MovingHorizonEstimator`](@ref). @@ -108,7 +110,9 @@ struct MovingHorizonEstimator{ corrected::Vector{Bool} buffer::StateEstimatorBuffer{NT} function MovingHorizonEstimator{NT}( - model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt, optim::JM, covestim::CE; + model::SM, + He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt, + optim::JM, gradient, jacobian, covestim::CE; direct=true ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}} nu, ny, nd = model.nu, model.ny, model.nd @@ -169,7 +173,7 @@ struct MovingHorizonEstimator{ direct, corrected, buffer ) - init_optimization!(estim, model, optim) + init_optimization!(estim, model, optim, gradient, jacobian) return estim end end @@ -256,6 +260,10 @@ transcription for now. - `optim=default_optim_mhe(model)` : a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model) with a quadratic/nonlinear optimizer for solving (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl), or [`OSQP`](https://osqp.org/docs/parsers/jump.html) if `model` is a [`LinModel`](@ref)). +- `gradient=AutoForwardDiff()` : an `AbstractADType` backend for the gradient of the objective + function if `model` is not a [`LinModel`](@ref), see [`DifferentiationInterface`](https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/explanation/backends/#List). +- `jacobian=AutoForwardDiff()` : an `AbstractADType` backend for the Jacobian of the + constraints if `model` is not a [`LinModel`](@ref), see `gradient` above for the options. - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current estimator, in opposition to the delayed/predictor form). @@ -365,6 +373,8 @@ function MovingHorizonEstimator( sigmaQint_ym = fill(1, max(sum(nint_ym), 0)), Cwt::Real = Inf, optim::JM = default_optim_mhe(model), + gradient::AbstractADType = DEFAULT_NONLINMJE_GRADIENT, + jacobian::AbstractADType = DEFAULT_NONLINMJE_JACOBIAN, direct = true, σP_0 = sigmaP_0, σQ = sigmaQ, @@ -380,7 +390,7 @@ function MovingHorizonEstimator( R̂ = Hermitian(diagm(NT[σR;].^2), :L) isnothing(He) && throw(ArgumentError("Estimation horizon He must be explicitly specified")) return MovingHorizonEstimator( - model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt; direct, optim + model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt; direct, optim, gradient, jacobian ) end @@ -391,6 +401,8 @@ default_optim_mhe(::SimModel) = JuMP.Model(DEFAULT_NONLINMHE_OPTIMIZER, add_brid MovingHorizonEstimator( model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf; optim=default_optim_mhe(model), + gradient=AutoForwardDiff(), + jacobian=AutoForwardDiff(), direct=true, covestim=default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) ) @@ -407,12 +419,17 @@ supported types are [`KalmanFilter`](@ref), [`UnscentedKalmanFilter`](@ref) and function MovingHorizonEstimator( model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf; optim::JM = default_optim_mhe(model), + gradient::AbstractADType = DEFAULT_NONLINMJE_GRADIENT, + jacobian::AbstractADType = DEFAULT_NONLINMJE_JACOBIAN, direct = true, covestim::CE = default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) return MovingHorizonEstimator{NT}( - model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂ , R̂, Cwt, optim, covestim; direct + model, + He, i_ym, nint_u, nint_ym, P̂_0, Q̂ , R̂, Cwt, + optim, gradient, jacobian, covestim; + direct ) end @@ -1179,12 +1196,12 @@ function init_predmat_mhe( end """ - init_optimization!(estim::MovingHorizonEstimator, model::SimModel, optim) + init_optimization!(estim::MovingHorizonEstimator, model::SimModel, optim, _ , _ ) Init the quadratic optimization of [`MovingHorizonEstimator`](@ref). """ function init_optimization!( - estim::MovingHorizonEstimator, ::LinModel, optim::JuMP.GenericModel + estim::MovingHorizonEstimator, ::LinModel, optim::JuMP.GenericModel, _ , _ ) nZ̃ = length(estim.Z̃) JuMP.num_variables(optim) == 0 || JuMP.empty!(optim) @@ -1199,12 +1216,22 @@ function init_optimization!( end """ - init_optimization!(estim::MovingHorizonEstimator, model::SimModel, optim) + init_optimization!( + estim::MovingHorizonEstimator, + model::SimModel, + optim::JuMP.GenericModel, + gradient::AbstractADType, + jacobian::AbstractADType + ) -> nothing Init the nonlinear optimization of [`MovingHorizonEstimator`](@ref). """ function init_optimization!( - estim::MovingHorizonEstimator, model::SimModel, optim::JuMP.GenericModel{JNT}, + estim::MovingHorizonEstimator, + model::SimModel, + optim::JuMP.GenericModel{JNT}, + gradient::AbstractADType, + jacobian::AbstractADType ) where JNT<:Real C, con = estim.C, estim.con nZ̃ = length(estim.Z̃) @@ -1225,7 +1252,9 @@ function init_optimization!( JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) end end - Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! = get_optim_functions(estim, optim) + Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! = get_optim_functions( + estim, optim, gradient, jacobian + ) @operator(optim, J, nZ̃, Jfunc, ∇Jfunc!) @objective(optim, Min, J(Z̃var...)) nV̂, nX̂ = estim.He*estim.nym, estim.He*estim.nx̂ @@ -1266,7 +1295,10 @@ end """ get_optim_functions( - estim::MovingHorizonEstimator, optim::JuMP.GenericModel + estim::MovingHorizonEstimator, + optim::JuMP.GenericModel, + grad_backend::AbstractADType, + jac_backend::AbstractADType ) -> Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! Return the functions for the nonlinear optimization of [`MovingHorizonEstimator`](@ref). @@ -1289,7 +1321,10 @@ This method is really intricate and I'm not proud of it. That's because of 3 ele Inspired from: [User-defined operators with vector outputs](https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs) """ function get_optim_functions( - estim::MovingHorizonEstimator, ::JuMP.GenericModel{JNT} + estim::MovingHorizonEstimator, + optim::JuMP.GenericModel{JNT}, + grad_backend::AbstractADType, + jac_backend::AbstractADType ) where {JNT <: Real} model, con = estim.model, estim.con nx̂, nym, nŷ, nu, nϵ, He = estim.nx̂, estim.nym, model.ny, model.nu, estim.nϵ, estim.He @@ -1336,19 +1371,19 @@ function get_optim_functions( x̄, V̂ = get_tmp(x̄_cache, T), get_tmp(V̂_cache, T) return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T end - Z̃_∇J = fill(myNaN, nZ̃) - ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J - ∇J_buffer = GradientBuffer(Jfunc_vec, Z̃_∇J) + Z̃_∇J = fill(myNaN, nZ̃) + ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J + ∇J_prep = prepare_gradient(Jfunc_vec, grad_backend, Z̃_∇J) ∇Jfunc! = if nZ̃ == 1 function (Z̃arg::T) where T<:Real Z̃_∇J .= Z̃arg - gradient!(∇J, ∇J_buffer, Z̃_∇J) + gradient!(Jfunc_vec, ∇J, ∇J_prep, grad_backend, Z̃_∇J) return ∇J[begin] # univariate syntax, see JuMP.@operator doc end else function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} Z̃_∇J .= Z̃arg - gradient!(∇J, ∇J_buffer, Z̃_∇J) + gradient!(Jfunc_vec, ∇J, ∇J_prep, grad_backend, Z̃_∇J) return ∇J # multivariate syntax, see JuMP.@operator doc end end @@ -1367,17 +1402,17 @@ function get_optim_functions( g .= get_tmp(g_cache, T) return g end - Z̃_∇g = fill(myNaN, nZ̃) - g_vec = Vector{JNT}(undef, ng) - ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of inequality constraints g - ∇g_buffer = JacobianBuffer(gfunc_vec!, g_vec, Z̃_∇g) - ∇gfuncs! = Vector{Function}(undef, ng) + Z̃_∇g = fill(myNaN, nZ̃) + g_vec = Vector{JNT}(undef, ng) + ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of inequality constraints g + ∇g_prep = prepare_jacobian(gfunc_vec!, g_vec, jac_backend, Z̃_∇g) + ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) ∇gfuncs![i] = if nZ̃ == 1 function (Z̃arg::T) where T<:Real if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!(∇g, ∇g_buffer, g_vec, Z̃_∇g) + jacobian!(gfunc_vec!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g) end return ∇g[i, begin] # univariate syntax, see JuMP.@operator doc end @@ -1385,7 +1420,7 @@ function get_optim_functions( function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!(∇g, ∇g_buffer, g_vec, Z̃_∇g) + jacobian!(gfunc_vec!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g) end return ∇g_i .= @views ∇g[i, :] # multivariate syntax, see JuMP.@operator doc end diff --git a/src/general.jl b/src/general.jl index 700c3f1c1..b9dff27c1 100644 --- a/src/general.jl +++ b/src/general.jl @@ -13,20 +13,6 @@ function Base.show(io::IO, buffer::DifferentiationBuffer) return print(io, "DifferentiationBuffer with a $(typeof(buffer.config).name.name)") end -"Struct with both function and configuration for ForwardDiff gradient." -struct GradientBuffer{FT<:Function, CT<:ForwardDiff.GradientConfig} <: DifferentiationBuffer - f::FT - config::CT -end - -"Create a GradientBuffer with function `f` and input `x`." -GradientBuffer(f, x) = GradientBuffer(f, ForwardDiff.GradientConfig(f, x)) - -"Compute in-place and return the gradient of `buffer.f` at `x`." -function gradient!(g, buffer::GradientBuffer, x) - return ForwardDiff.gradient!(g, buffer.f, x, buffer.config) -end - "Struct with both function and configuration for ForwardDiff Jacobian." struct JacobianBuffer{FT<:Function, CT<:ForwardDiff.JacobianConfig} <: DifferentiationBuffer f!::FT @@ -37,7 +23,7 @@ end JacobianBuffer(f!, y, x) = JacobianBuffer(f!, ForwardDiff.JacobianConfig(f!, y, x)) "Compute in-place and return the Jacobian matrix of `buffer.f!` at `x`." -function jacobian!(A, buffer::JacobianBuffer, y, x) +function get_jacobian!(A, buffer::JacobianBuffer, y, x) return ForwardDiff.jacobian!(A, buffer.f!, y, x, buffer.config) end diff --git a/src/model/linearization.jl b/src/model/linearization.jl index 6fc47130c..efcb64b5e 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -209,11 +209,11 @@ function get_jacobians!(linmodel::LinModel, xnext0, y0, model::SimModel, x0, u0, linbuffer.x .= x0 linbuffer.u .= u0 linbuffer.d .= d0 - jacobian!(linmodel.A, linbuffer.buffer_f_at_u_d, xnext0, x0) - jacobian!(linmodel.Bu, linbuffer.buffer_f_at_x_d, xnext0, u0) - jacobian!(linmodel.Bd, linbuffer.buffer_f_at_x_u, xnext0, d0) - jacobian!(linmodel.C, linbuffer.buffer_h_at_d, y0, x0) - jacobian!(linmodel.Dd, linbuffer.buffer_h_at_x, y0, d0) + get_jacobian!(linmodel.A, linbuffer.buffer_f_at_u_d, xnext0, x0) + get_jacobian!(linmodel.Bu, linbuffer.buffer_f_at_x_d, xnext0, u0) + get_jacobian!(linmodel.Bd, linbuffer.buffer_f_at_x_u, xnext0, d0) + get_jacobian!(linmodel.C, linbuffer.buffer_h_at_d, y0, x0) + get_jacobian!(linmodel.Dd, linbuffer.buffer_h_at_x, y0, d0) return nothing end From a4ca9b8a283274daf5baa71682070053081e9ceb Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 12 Mar 2025 17:44:50 -0400 Subject: [PATCH 02/30] debug: typo in MHE --- src/estimator/mhe/construct.jl | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 90f818fa6..38ce33fc7 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -373,8 +373,8 @@ function MovingHorizonEstimator( sigmaQint_ym = fill(1, max(sum(nint_ym), 0)), Cwt::Real = Inf, optim::JM = default_optim_mhe(model), - gradient::AbstractADType = DEFAULT_NONLINMJE_GRADIENT, - jacobian::AbstractADType = DEFAULT_NONLINMJE_JACOBIAN, + gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT, + jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN, direct = true, σP_0 = sigmaP_0, σQ = sigmaQ, @@ -419,8 +419,8 @@ supported types are [`KalmanFilter`](@ref), [`UnscentedKalmanFilter`](@ref) and function MovingHorizonEstimator( model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf; optim::JM = default_optim_mhe(model), - gradient::AbstractADType = DEFAULT_NONLINMJE_GRADIENT, - jacobian::AbstractADType = DEFAULT_NONLINMJE_JACOBIAN, + gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT, + jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN, direct = true, covestim::CE = default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}} From 56433fb7938314e003470be57c37375d9ea8d611 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 13 Mar 2025 08:21:10 -0400 Subject: [PATCH 03/30] starting support of sparse Jacobians --- Project.toml | 4 ++++ src/ModelPredictiveControl.jl | 10 +++------- src/controller/nonlinmpc.jl | 12 +++++++++--- src/estimator/mhe/construct.jl | 10 ++++++++-- 4 files changed, 24 insertions(+), 12 deletions(-) diff --git a/Project.toml b/Project.toml index 6c3efad23..bcbf31b4b 100644 --- a/Project.toml +++ b/Project.toml @@ -17,6 +17,8 @@ PrecompileTools = "aea7be01-6a6a-4083-8856-8a6e6704d82a" ProgressLogging = "33c8b6b6-d38a-422a-b730-caa89a2f386c" Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" RecipesBase = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" +SparseConnectivityTracer = "9f842d2f-2579-4b1d-911e-f412cf18a3f5" +SparseMatrixColorings = "0a514795-09f3-496d-8182-132a7b665d35" [compat] ControlSystemsBase = "1.9" @@ -32,6 +34,8 @@ PrecompileTools = "1" ProgressLogging = "0.1" Random = "1.10" RecipesBase = "1" +SparseConnectivityTracer = "0.6.13" +SparseMatrixColorings = "0.4.14" julia = "1.10" [extras] diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index fdcd90b33..daf47986b 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -7,16 +7,12 @@ using Random: randn using RecipesBase using ProgressLogging - - using DifferentiationInterface: ADTypes.AbstractADType, AutoForwardDiff, AutoSparse using DifferentiationInterface: gradient!, jacobian!, prepare_gradient, prepare_jacobian +using SparseConnectivityTracer: TracerSparsityDetector +using SparseMatrixColorings: GreedyColoringAlgorithm -import ForwardDiff - - - - +import ForwardDiff #TODO: delete this after `linearize!` and `ExtendedKalmanFilter` are updated import ControlSystemsBase import ControlSystemsBase: ss, tf, delay diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 1b85a23ad..b883f1cea 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -1,6 +1,12 @@ const DEFAULT_NONLINMPC_TRANSCRIPTION = SingleShooting() const DEFAULT_NONLINMPC_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes") -const DEFAULT_NONLINMPC_GRADIENT = AutoForwardDiff() +const DEFAULT_NONLINMPC_GRADIENT = AutoForwardDiff() +const DEFAULT_NONLINMPC_JACDENSE = AutoForwardDiff() +const DEFAULT_NONLINMPC_JACSPARSE = AutoSparse( + AutoForwardDiff(); + sparsity_detector=TracerSparsityDetector(), + coloring_algorithm=GreedyColoringAlgorithm(), +) struct NonLinMPC{ NT<:Real, @@ -375,8 +381,8 @@ function NonLinMPC( ) end -default_jacobian(::SingleShooting) = AutoForwardDiff() -default_jacobian(::TranscriptionMethod) = AutoForwardDiff() +default_jacobian(::SingleShooting) = DEFAULT_NONLINMPC_JACDENSE +default_jacobian(::TranscriptionMethod) = DEFAULT_NONLINMPC_JACSPARSE """ validate_JE(NT, JE) -> nothing diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 38ce33fc7..484156e9d 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1196,12 +1196,18 @@ function init_predmat_mhe( end """ - init_optimization!(estim::MovingHorizonEstimator, model::SimModel, optim, _ , _ ) + init_optimization!( + estim::MovingHorizonEstimator, model::SimModel, optim::JuMP.GenericModel, _ , _ + ) Init the quadratic optimization of [`MovingHorizonEstimator`](@ref). """ function init_optimization!( - estim::MovingHorizonEstimator, ::LinModel, optim::JuMP.GenericModel, _ , _ + estim::MovingHorizonEstimator, + ::LinModel, + optim::JuMP.GenericModel, + ::AbstractADType, + ::AbstractADType ) nZ̃ = length(estim.Z̃) JuMP.num_variables(optim) == 0 || JuMP.empty!(optim) From 6887c62a7ef90cae8539b5d1518f0becc8e71c02 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 13 Mar 2025 18:04:21 -0400 Subject: [PATCH 04/30] WIP: using `Cache` instead of `DiffCache` --- src/ModelPredictiveControl.jl | 1 + src/controller/nonlinmpc.jl | 179 +++++++++++++++++++++------------- 2 files changed, 114 insertions(+), 66 deletions(-) diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index daf47986b..0a447c706 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -9,6 +9,7 @@ using ProgressLogging using DifferentiationInterface: ADTypes.AbstractADType, AutoForwardDiff, AutoSparse using DifferentiationInterface: gradient!, jacobian!, prepare_gradient, prepare_jacobian +using DifferentiationInterface: Constant, Cache using SparseConnectivityTracer: TracerSparsityDetector using SparseMatrixColorings: GreedyColoringAlgorithm diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index b883f1cea..16f42528d 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -582,43 +582,14 @@ function get_optim_functions( jac_backend ::AbstractADType ) where JNT<:Real model, transcription = mpc.estim.model, mpc.transcription - nu, ny, nx̂, nϵ, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, mpc.Hp, mpc.Hc - ng, nc, neq = length(mpc.con.i_g), mpc.con.nc, mpc.con.neq - nZ̃, nU, nŶ, nX̂ = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂ - nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny - Ncache = nZ̃ + 3 - myNaN = convert(JNT, NaN) # fill Z̃ with NaNs to force update_simulations! at 1st call: - # ---------------------- differentiation cache --------------------------------------- - Z̃_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(fill(myNaN, nZ̃), Ncache) - ΔŨ_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nΔŨ), Ncache) - x̂0end_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Ncache) - Ŷe_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŶe), Ncache) - Ue_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nUe), Ncache) - Ŷ0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŶ), Ncache) - U0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nU), Ncache) - Û0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nU), Ncache) - X̂0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nX̂), Ncache) - gc_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nc), Ncache) - g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Ncache) - geq_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, neq), Ncache) # --------------------- update simulation function ------------------------------------ - function update_simulations!( - Z̃arg::Union{NTuple{N, T}, AbstractVector{T}}, Z̃cache - ) where {N, T<:Real} - if isdifferent(Z̃cache, Z̃arg) - for i in eachindex(Z̃cache) + function update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + if isdifferent(Z̃arg, Z̃) + for i in eachindex(Z̃) # Z̃cache .= Z̃arg is type unstable with Z̃arg::NTuple{N, FowardDiff.Dual} - Z̃cache[i] = Z̃arg[i] + Z̃[i] = Z̃arg[i] end - Z̃ = Z̃cache ϵ = (nϵ ≠ 0) ? Z̃[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation) - ΔŨ = get_tmp(ΔŨ_cache, T) - x̂0end = get_tmp(x̂0end_cache, T) - Ue, Ŷe = get_tmp(Ue_cache, T), get_tmp(Ŷe_cache, T) - U0, Ŷ0 = get_tmp(U0_cache, T), get_tmp(Ŷ0_cache, T) - X̂0, Û0 = get_tmp(X̂0_cache, T), get_tmp(Û0_cache, T) - gc, g = get_tmp(gc_cache, T), get_tmp(g_cache, T) - geq = get_tmp(geq_cache, T) U0 = getU0!(U0, mpc, Z̃) ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃) @@ -629,63 +600,133 @@ function get_optim_functions( end return nothing end - # --------------------- objective functions ------------------------------------------- + # ---------------------- JNT vectors cache -------------------------------------------- + nu, ny, nx̂, nϵ, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, mpc.Hp, mpc.Hc + ng, nc, neq = length(mpc.con.i_g), mpc.con.nc, mpc.con.neq + nZ̃, nU, nŶ, nX̂ = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂ + nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny + myNaN = convert(JNT, NaN) + Z̃ = fill(myNaN, nZ̃) + ΔŨ = zeros(JNT, nΔŨ) + x̂0end = zeros(JNT, nx̂) + Ue, Ŷe = zeros(JNT, nUe), zeros(JNT, nŶe) + U0, Ŷ0 = zeros(JNT, nU), zeros(JNT, nŶ) + Û0, X̂0 = zeros(JNT, nU), zeros(JNT, nX̂) + gc, g = zeros(JNT, nc), zeros(JNT, ng) + geq = zeros(JNT, neq) + + # WIP: still receive INVALID_MODEL once and a while, needs to investigate. + + function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} - update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) - ΔŨ = get_tmp(ΔŨ_cache, T) - Ue, Ŷe = get_tmp(Ue_cache, T), get_tmp(Ŷe_cache, T) - U0, Ŷ0 = get_tmp(U0_cache, T), get_tmp(Ŷ0_cache, T) + update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T end - function Jfunc_vec(Z̃arg::AbstractVector{T}) where T<:Real - update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) - ΔŨ = get_tmp(ΔŨ_cache, T) - Ue, Ŷe = get_tmp(Ue_cache, T), get_tmp(Ŷe_cache, T) - U0, Ŷ0 = get_tmp(U0_cache, T), get_tmp(Ŷ0_cache, T) - return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T + function Jfunc_vec!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) end + + + + gfuncs = Vector{Function}(undef, ng) + for i in eachindex(gfuncs) + func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} + update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + return g[i]::T + end + gfuncs[i] = func_i + end + function gfunc_vec!(g, Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, geq) + update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + return g + end + + + + + + + + + Z̃_∇J = fill(myNaN, nZ̃) + ΔŨ_∇J = zeros(JNT, nΔŨ) + x̂0end_∇J = zeros(JNT, nx̂) + Ue_∇J, Ŷe_∇J = zeros(JNT, nUe), zeros(JNT, nŶe) + U0_∇J, Ŷ0_∇J = zeros(JNT, nU), zeros(JNT, nŶ) + Û0_∇J, X̂0_∇J = zeros(JNT, nU), zeros(JNT, nX̂) + gc_∇J, g_∇J = zeros(JNT, nc), zeros(JNT, ng) + geq_∇J = zeros(JNT, neq) + + + + + + + Z̃_∇J = fill(myNaN, nZ̃) ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J - ∇J_prep = prepare_gradient(Jfunc_vec, grad_backend, Z̃_∇J) + ∇J_context = ( + Cache(Z̃_∇J), Cache(ΔŨ_∇J), Cache(x̂0end_∇J), + Cache(Ue_∇J), Cache(Ŷe_∇J), + Cache(U0_∇J), Cache(Ŷ0_∇J), + Cache(Û0_∇J), Cache(X̂0_∇J), + Cache(gc_∇J), Cache(g_∇J), Cache(geq_∇J) + ) + ∇J_prep = prepare_gradient(Jfunc_vec!, grad_backend, Z̃_∇J, ∇J_context...) ∇Jfunc! = if nZ̃ == 1 - function (Z̃arg::T) where T<:Real + function (Z̃arg) Z̃_∇J .= Z̃arg - gradient!(Jfunc_vec, ∇J, ∇J_prep, grad_backend, Z̃_∇J) + gradient!(Jfunc_vec!, ∇J, ∇J_prep, grad_backend, Z̃_∇J, ∇J_context...) return ∇J[begin] # univariate syntax, see JuMP.@operator doc end else function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} Z̃_∇J .= Z̃arg - gradient!(Jfunc_vec, ∇J, ∇J_prep, grad_backend, Z̃_∇J) + gradient!(Jfunc_vec!, ∇J, ∇J_prep, grad_backend, Z̃_∇J, ∇J_context...) return ∇J # multivariate syntax, see JuMP.@operator doc end end - # --------------------- inequality constraint functions ------------------------------- - gfuncs = Vector{Function}(undef, ng) - for i in eachindex(gfuncs) - func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} - update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) - g = get_tmp(g_cache, T) - return g[i]::T - end - gfuncs[i] = func_i - end - function gfunc_vec!(g, Z̃vec::AbstractVector{T}) where T<:Real - update_simulations!(Z̃vec, get_tmp(Z̃_cache, T)) - g .= get_tmp(g_cache, T) - return g - end + + + + + + + + Z̃_∇g = fill(myNaN, nZ̃) + ΔŨ_∇g = zeros(JNT, nΔŨ) + x̂0end_∇g = zeros(JNT, nx̂) + Ue_∇g, Ŷe_∇g = zeros(JNT, nUe), zeros(JNT, nŶe) + U0_∇g, Ŷ0_∇g = zeros(JNT, nU), zeros(JNT, nŶ) + Û0_∇g, X̂0_∇g = zeros(JNT, nU), zeros(JNT, nX̂) + gc_∇g, g_∇g = zeros(JNT, nc), zeros(JNT, ng) + geq_∇g = zeros(JNT, neq) + + + + + Z̃_∇g = fill(myNaN, nZ̃) g_vec = Vector{JNT}(undef, ng) ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of inequality constraints g - ∇g_prep = prepare_jacobian(gfunc_vec!, g_vec, jac_backend, Z̃_∇g) + ∇g_context = ( + Cache(Z̃_∇g), Cache(ΔŨ_∇g), Cache(x̂0end_∇g), + Cache(Ue_∇g), Cache(Ŷe_∇g), + Cache(U0_∇g), Cache(Ŷ0_∇g), + Cache(Û0_∇g), Cache(X̂0_∇g), + Cache(gc_∇g), Cache(geq_∇g) + ) + ∇g_prep = prepare_jacobian(gfunc_vec!, g_vec, jac_backend, Z̃_∇g, ∇g_context...) ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) ∇gfuncs![i] = if nZ̃ == 1 function (Z̃arg::T) where T<:Real if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!(gfunc_vec!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g) + jacobian!( + gfunc_vec!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context... + ) end return ∇g[i, begin] # univariate syntax, see JuMP.@operator doc end @@ -693,7 +734,9 @@ function get_optim_functions( function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!(gfunc_vec!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g) + jacobian!( + gfunc_vec!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context... + ) end return ∇g_i .= @views ∇g[i, :] # multivariate syntax, see JuMP.@operator doc end @@ -701,6 +744,7 @@ function get_optim_functions( end # --------------------- equality constraint functions --------------------------------- geqfuncs = Vector{Function}(undef, neq) + #= for i in eachindex(geqfuncs) func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) @@ -718,7 +762,9 @@ function get_optim_functions( geq_vec = Vector{JNT}(undef, neq) ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of equality constraints geq ∇geq_prep = prepare_jacobian(geqfunc_vec!, geq_vec, jac_backend, Z̃_∇geq) + =# ∇geqfuncs! = Vector{Function}(undef, neq) + #= for i in eachindex(∇geqfuncs!) # only multivariate syntax, univariate is impossible since nonlinear equality # constraints imply MultipleShooting, thus input increment ΔU and state X̂0 in Z̃: @@ -731,6 +777,7 @@ function get_optim_functions( return ∇geq_i .= @views ∇geq[i, :] end end + =# return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! end From 361da2b92bc069d7ee95549a3d2878744af69bd9 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 15 Mar 2025 13:03:29 -0400 Subject: [PATCH 05/30] added: compat on `DI.jl` --- Project.toml | 2 +- src/controller/execute.jl | 11 ++- src/controller/nonlinmpc.jl | 138 +++++++++++++----------------------- 3 files changed, 59 insertions(+), 92 deletions(-) diff --git a/Project.toml b/Project.toml index bcbf31b4b..57b28b181 100644 --- a/Project.toml +++ b/Project.toml @@ -22,7 +22,7 @@ SparseMatrixColorings = "0a514795-09f3-496d-8182-132a7b665d35" [compat] ControlSystemsBase = "1.9" -DifferentiationInterface = "0.6.43" +DifferentiationInterface = "0.6.44" ForwardDiff = "0.10" Ipopt = "1" JuMP = "1.21" diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 7becca12f..cd327203a 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -135,7 +135,7 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real J = obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) predictstoch!(Ŷs, mpc, mpc.estim) info[:ΔU] = Z̃[1:mpc.Hc*model.nu] - info[:ϵ] = mpc.nϵ == 1 ? mpc.Z̃[end] : zero(NT) + info[:ϵ] = getϵ(mpc, Z̃) info[:J] = J info[:U] = U info[:u] = info[:U][1:model.nu] @@ -161,6 +161,15 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real return info end +""" + getϵ(mpc::PredictiveController, Z̃) -> ϵ + +Get the slack `ϵ` from the decision vector `Z̃` if present, otherwise return 0. +""" +function getϵ(mpc::PredictiveController, Z̃::AbstractVector{NT}) where NT<:Real + return mpc.nϵ ≠ 0 ? Z̃[end] : zero(NT) +end + """ addinfo!(info, mpc::PredictiveController) -> info diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 16f42528d..d385d5281 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -382,7 +382,7 @@ function NonLinMPC( end default_jacobian(::SingleShooting) = DEFAULT_NONLINMPC_JACDENSE -default_jacobian(::TranscriptionMethod) = DEFAULT_NONLINMPC_JACSPARSE +default_jacobian(::TranscriptionMethod) = DEFAULT_NONLINMPC_JACDENSE #TODO: DEFAULT_NONLINMPC_JACSPARSE """ validate_JE(NT, JE) -> nothing @@ -589,11 +589,11 @@ function get_optim_functions( # Z̃cache .= Z̃arg is type unstable with Z̃arg::NTuple{N, FowardDiff.Dual} Z̃[i] = Z̃arg[i] end - ϵ = (nϵ ≠ 0) ? Z̃[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation) U0 = getU0!(U0, mpc, Z̃) ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃) Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) + ϵ = getϵ(mpc, Z̃) gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ) g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ) geq = con_nonlinprogeq!(geq, X̂0, Û0, mpc, model, transcription, U0, Z̃) @@ -615,9 +615,8 @@ function get_optim_functions( gc, g = zeros(JNT, nc), zeros(JNT, ng) geq = zeros(JNT, neq) - # WIP: still receive INVALID_MODEL once and a while, needs to investigate. - + # ---------------------- objective function ------------------------------------------ function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T @@ -627,51 +626,14 @@ function get_optim_functions( return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) end - - - gfuncs = Vector{Function}(undef, ng) - for i in eachindex(gfuncs) - func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} - update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) - return g[i]::T - end - gfuncs[i] = func_i - end - function gfunc_vec!(g, Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, geq) - update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) - return g - end - - - - - - - - - Z̃_∇J = fill(myNaN, nZ̃) - ΔŨ_∇J = zeros(JNT, nΔŨ) - x̂0end_∇J = zeros(JNT, nx̂) - Ue_∇J, Ŷe_∇J = zeros(JNT, nUe), zeros(JNT, nŶe) - U0_∇J, Ŷ0_∇J = zeros(JNT, nU), zeros(JNT, nŶ) - Û0_∇J, X̂0_∇J = zeros(JNT, nU), zeros(JNT, nX̂) - gc_∇J, g_∇J = zeros(JNT, nc), zeros(JNT, ng) - geq_∇J = zeros(JNT, neq) - - - - - - - Z̃_∇J = fill(myNaN, nZ̃) ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J ∇J_context = ( - Cache(Z̃_∇J), Cache(ΔŨ_∇J), Cache(x̂0end_∇J), - Cache(Ue_∇J), Cache(Ŷe_∇J), - Cache(U0_∇J), Cache(Ŷ0_∇J), - Cache(Û0_∇J), Cache(X̂0_∇J), - Cache(gc_∇J), Cache(g_∇J), Cache(geq_∇J) + Cache(Z̃), Cache(ΔŨ), Cache(x̂0end), + Cache(Ue), Cache(Ŷe), + Cache(U0), Cache(Ŷ0), + Cache(Û0), Cache(X̂0), + Cache(gc), Cache(g), Cache(geq) ) ∇J_prep = prepare_gradient(Jfunc_vec!, grad_backend, Z̃_∇J, ∇J_context...) ∇Jfunc! = if nZ̃ == 1 @@ -689,44 +651,38 @@ function get_optim_functions( end - - - - + # --------------------- inequality constraint functions ------------------------------- + gfuncs = Vector{Function}(undef, ng) + for i in eachindex(gfuncs) + gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} + update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + return g[i]::T + end + gfuncs[i] = gfunc_i + end + function gfunc!(g, Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, geq) + update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + return g + end Z̃_∇g = fill(myNaN, nZ̃) - ΔŨ_∇g = zeros(JNT, nΔŨ) - x̂0end_∇g = zeros(JNT, nx̂) - Ue_∇g, Ŷe_∇g = zeros(JNT, nUe), zeros(JNT, nŶe) - U0_∇g, Ŷ0_∇g = zeros(JNT, nU), zeros(JNT, nŶ) - Û0_∇g, X̂0_∇g = zeros(JNT, nU), zeros(JNT, nX̂) - gc_∇g, g_∇g = zeros(JNT, nc), zeros(JNT, ng) - geq_∇g = zeros(JNT, neq) - - - - - - Z̃_∇g = fill(myNaN, nZ̃) g_vec = Vector{JNT}(undef, ng) ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of inequality constraints g ∇g_context = ( - Cache(Z̃_∇g), Cache(ΔŨ_∇g), Cache(x̂0end_∇g), - Cache(Ue_∇g), Cache(Ŷe_∇g), - Cache(U0_∇g), Cache(Ŷ0_∇g), - Cache(Û0_∇g), Cache(X̂0_∇g), - Cache(gc_∇g), Cache(geq_∇g) + Cache(Z̃), Cache(ΔŨ), Cache(x̂0end), + Cache(Ue), Cache(Ŷe), + Cache(U0), Cache(Ŷ0), + Cache(Û0), Cache(X̂0), + Cache(gc), Cache(geq) ) - ∇g_prep = prepare_jacobian(gfunc_vec!, g_vec, jac_backend, Z̃_∇g, ∇g_context...) + ∇g_prep = prepare_jacobian(gfunc!, g_vec, jac_backend, Z̃_∇g, ∇g_context...) ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) - ∇gfuncs![i] = if nZ̃ == 1 + ∇gfuncs_i! = if nZ̃ == 1 function (Z̃arg::T) where T<:Real if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!( - gfunc_vec!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context... - ) + jacobian!(gfunc!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context...) end return ∇g[i, begin] # univariate syntax, see JuMP.@operator doc end @@ -734,50 +690,52 @@ function get_optim_functions( function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!( - gfunc_vec!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context... - ) + jacobian!(gfunc!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context...) end return ∇g_i .= @views ∇g[i, :] # multivariate syntax, see JuMP.@operator doc end end + ∇gfuncs![i] = ∇gfuncs_i! end # --------------------- equality constraint functions --------------------------------- geqfuncs = Vector{Function}(undef, neq) - #= for i in eachindex(geqfuncs) - func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} - update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) - geq = get_tmp(geq_cache, T) + geqfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} + update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) return geq[i]::T end - geqfuncs[i] = func_i + geqfuncs[i] = geqfunc_i end - function geqfunc_vec!(geq, Z̃vec::AbstractVector{T}) where T<:Real - update_simulations!(Z̃vec, get_tmp(Z̃_cache, T)) - geq .= get_tmp(geq_cache, T) + function geqfunc!(geq, Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g) + update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) return geq end + Z̃_∇geq = fill(myNaN, nZ̃) geq_vec = Vector{JNT}(undef, neq) ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of equality constraints geq - ∇geq_prep = prepare_jacobian(geqfunc_vec!, geq_vec, jac_backend, Z̃_∇geq) - =# + ∇geq_context = ( + Cache(Z̃), Cache(ΔŨ), Cache(x̂0end), + Cache(Ue), Cache(Ŷe), + Cache(U0), Cache(Ŷ0), + Cache(Û0), Cache(X̂0), + Cache(gc), Cache(g) + ) + ∇geq_prep = prepare_jacobian(geqfunc!, geq_vec, jac_backend, Z̃_∇geq, ∇geq_context...) ∇geqfuncs! = Vector{Function}(undef, neq) - #= for i in eachindex(∇geqfuncs!) # only multivariate syntax, univariate is impossible since nonlinear equality # constraints imply MultipleShooting, thus input increment ΔU and state X̂0 in Z̃: - ∇geqfuncs![i] = + ∇geqfuncs_i! = function (∇geq_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃_∇geq) Z̃_∇geq .= Z̃arg - jacobian!(geqfunc_vec!, geq_vec, ∇geq, ∇geq_prep, jac_backend, Z̃_∇geq) + jacobian!(geqfunc!, geq_vec, ∇geq, ∇geq_prep, jac_backend, Z̃_∇geq, ∇geq_context...) end return ∇geq_i .= @views ∇geq[i, :] end + ∇geqfuncs![i] = ∇geqfuncs_i! end - =# return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! end From 574b0a351de54bea1e5c4ac28237d6a522a7ca62 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 16 Mar 2025 20:45:32 -0400 Subject: [PATCH 06/30] wip: support for sparse jacobians --- src/controller/nonlinmpc.jl | 52 +++++++++++++++++++------------------ 1 file changed, 27 insertions(+), 25 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index ecb75e0ee..9577507cd 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -382,7 +382,7 @@ function NonLinMPC( end default_jacobian(::SingleShooting) = DEFAULT_NONLINMPC_JACDENSE -default_jacobian(::TranscriptionMethod) = DEFAULT_NONLINMPC_JACDENSE #TODO: DEFAULT_NONLINMPC_JACSPARSE +default_jacobian(::TranscriptionMethod) = DEFAULT_NONLINMPC_JACSPARSE """ validate_JE(NT, JE) -> nothing @@ -605,7 +605,7 @@ function get_optim_functions( ng, nc, neq = length(mpc.con.i_g), mpc.con.nc, mpc.con.neq nZ̃, nU, nŶ, nX̂ = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂ nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny - myNaN = convert(JNT, NaN) + myNaN = convert(JNT, NaN) Z̃ = fill(myNaN, nZ̃) ΔŨ = zeros(JNT, nΔŨ) x̂0end = zeros(JNT, nx̂) @@ -614,20 +614,17 @@ function get_optim_functions( Û0, X̂0 = zeros(JNT, nU), zeros(JNT, nX̂) gc, g = zeros(JNT, nc), zeros(JNT, ng) geq = zeros(JNT, neq) - - # ---------------------- objective function ------------------------------------------ function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T end - function Jfunc_vec!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + function Jfunc!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) end - Z̃_∇J = fill(myNaN, nZ̃) - ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J + ∇J = zeros(JNT, nZ̃) # gradient of objective J ∇J_context = ( Cache(Z̃), Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), @@ -635,22 +632,20 @@ function get_optim_functions( Cache(Û0), Cache(X̂0), Cache(gc), Cache(g), Cache(geq) ) - ∇J_prep = prepare_gradient(Jfunc_vec!, grad_backend, Z̃_∇J, ∇J_context...) + ∇J_prep = prepare_gradient(Jfunc!, grad_backend, Z̃_∇J, ∇J_context...) ∇Jfunc! = if nZ̃ == 1 function (Z̃arg) Z̃_∇J .= Z̃arg - gradient!(Jfunc_vec!, ∇J, ∇J_prep, grad_backend, Z̃_∇J, ∇J_context...) + gradient!(Jfunc!, ∇J, ∇J_prep, grad_backend, Z̃_∇J, ∇J_context...) return ∇J[begin] # univariate syntax, see JuMP.@operator doc end else function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} Z̃_∇J .= Z̃arg - gradient!(Jfunc_vec!, ∇J, ∇J_prep, grad_backend, Z̃_∇J, ∇J_context...) + gradient!(Jfunc!, ∇J, ∇J_prep, grad_backend, Z̃_∇J, ∇J_context...) return ∇J # multivariate syntax, see JuMP.@operator doc end end - - # --------------------- inequality constraint functions ------------------------------- gfuncs = Vector{Function}(undef, ng) for i in eachindex(gfuncs) @@ -664,10 +659,8 @@ function get_optim_functions( update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) return g end - - Z̃_∇g = fill(myNaN, nZ̃) - g_vec = Vector{JNT}(undef, ng) - ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of inequality constraints g + Z̃_∇g = fill(myNaN, nZ̃) + ∇g = zeros(JNT, ng, nZ̃) # Jacobian of inequality constraints g ∇g_context = ( Cache(Z̃), Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), @@ -675,14 +668,22 @@ function get_optim_functions( Cache(Û0), Cache(X̂0), Cache(gc), Cache(geq) ) - ∇g_prep = prepare_jacobian(gfunc!, g_vec, jac_backend, Z̃_∇g, ∇g_context...) + + + # TODO: cleanup the next part: + i_g_old = copy(mpc.con.i_g) + mpc.con.i_g .= true + ∇g_prep = prepare_jacobian(gfunc!, g, jac_backend, Z̃_∇g, ∇g_context...) + mpc.con.i_g .= i_g_old + + ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) ∇gfuncs_i! = if nZ̃ == 1 function (Z̃arg::T) where T<:Real if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!(gfunc!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context...) + jacobian!(gfunc!, g, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context...) end return ∇g[i, begin] # univariate syntax, see JuMP.@operator doc end @@ -690,7 +691,8 @@ function get_optim_functions( function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!(gfunc!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context...) + jacobian!(gfunc!, g, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context...) + display(∇g) end return ∇g_i .= @views ∇g[i, :] # multivariate syntax, see JuMP.@operator doc end @@ -710,10 +712,8 @@ function get_optim_functions( update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) return geq end - - Z̃_∇geq = fill(myNaN, nZ̃) - geq_vec = Vector{JNT}(undef, neq) - ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of equality constraints geq + Z̃_∇geq = fill(myNaN, nZ̃) + ∇geq = zeros(JNT, neq, nZ̃) # Jacobian of equality constraints geq ∇geq_context = ( Cache(Z̃), Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), @@ -721,7 +721,7 @@ function get_optim_functions( Cache(Û0), Cache(X̂0), Cache(gc), Cache(g) ) - ∇geq_prep = prepare_jacobian(geqfunc!, geq_vec, jac_backend, Z̃_∇geq, ∇geq_context...) + ∇geq_prep = prepare_jacobian(geqfunc!, geq, jac_backend, Z̃_∇geq, ∇geq_context...) ∇geqfuncs! = Vector{Function}(undef, neq) for i in eachindex(∇geqfuncs!) # only multivariate syntax, univariate is impossible since nonlinear equality @@ -730,7 +730,9 @@ function get_optim_functions( function (∇geq_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃_∇geq) Z̃_∇geq .= Z̃arg - jacobian!(geqfunc!, geq_vec, ∇geq, ∇geq_prep, jac_backend, Z̃_∇geq, ∇geq_context...) + jacobian!( + geqfunc!, geq, ∇geq, ∇geq_prep, jac_backend, Z̃_∇geq, ∇geq_context... + ) end return ∇geq_i .= @views ∇geq[i, :] end From 7c41bc5f22dc3c1198327c2601fc08e34cf2e570 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 17 Mar 2025 11:43:46 -0400 Subject: [PATCH 07/30] doc: long url separated from the text --- src/ModelPredictiveControl.jl | 2 +- src/controller/nonlinmpc.jl | 111 ++++++++++++++++++--------------- src/estimator/kalman.jl | 9 ++- src/estimator/mhe/construct.jl | 8 ++- src/model/linearization.jl | 4 +- src/model/linmodel.jl | 13 ++-- src/model/nonlinmodel.jl | 5 +- 7 files changed, 87 insertions(+), 65 deletions(-) diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index 9a7875603..c7b844a04 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -1,6 +1,6 @@ module ModelPredictiveControl -using PrecompileTools +using PrecompileTools # TODO: remove this dep if possible (with Cache of DI.jl) using LinearAlgebra using Random: randn diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 9577507cd..518c20106 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -197,12 +197,14 @@ This controller allocates memory at each time step for the optimization. controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model) (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl) optimizer). - `gradient=AutoForwardDiff()` : an `AbstractADType` backend for the gradient of the objective - function, see [`DifferentiationInterface`](https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/explanation/backends/#List). + function, see [`DifferentiationInterface`][1]. - `jacobian=default_jacobian(transcription)` : an `AbstractADType` backend for the Jacobian of the nonlinear constraints, see `gradient` above for the options (default in Extended Help). - additional keyword arguments are passed to [`UnscentedKalmanFilter`](@ref) constructor (or [`SteadyKalmanFilter`](@ref), for [`LinModel`](@ref)). +[1]: https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/explanation/backends/#List + # Examples ```jldoctest julia> model = NonLinModel((x,u,_,_)->0.5x+u, (x,_,_)->2x, 10.0, 1, 1, 1, solver=nothing); @@ -248,14 +250,26 @@ NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedK The keyword argument `nc` is the number of elements in `LHS`, and `gc!`, an alias for the `gc` argument (both `gc` and `gc!` accepts non-mutating and mutating functions). - The optimization relies on [`JuMP`](https://github.com/jump-dev/JuMP.jl) automatic - differentiation (AD) to compute the objective and constraint derivatives. Optimizers - generally benefit from exact derivatives like AD. However, the [`NonLinModel`](@ref) - state-space functions must be compatible with this feature. See [Automatic differentiation](https://jump.dev/JuMP.jl/stable/manual/nlp/#Automatic-differentiation) + By default, the optimization relies on dense [`ForwardDiff`](https://github.com/JuliaDiff/ForwardDiff.jl) + automatic differentiation (AD) to compute the objective and constraint derivatives. One + exception: if `transcription` is not a [`SingleShooting`](@ref), the `jacobian` argument + defaults to this [sparse backend][2]: + ```julia + AutoSparse( + AutoForwardDiff(); + sparsity_detector = TracerSparsityDetector(), + coloring_algorithm = GreedyColoringAlgorithm() + ) + ``` + Optimizers generally benefit from exact derivatives like AD. However, the [`NonLinModel`](@ref) + state-space functions must be compatible with this feature. See `JuMP` [documentation][3] for common mistakes when writing these functions. Note that if `Cwt≠Inf`, the attribute `nlp_scaling_max_gradient` of `Ipopt` is set to `10/Cwt` (if not already set), to scale the small values of ``ϵ``. + + [2]: https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/explanation/advanced/#Sparsity + [3]: https://jump.dev/JuMP.jl/stable/manual/nonlinear/#Common-mistakes-when-writing-a-user-defined-operator """ function NonLinMPC( model::SimModel; @@ -567,13 +581,16 @@ This method is really intricate and I'm not proud of it. That's because of 3 ele - These functions are used inside the nonlinear optimization, so they must be type-stable and as efficient as possible. - The `JuMP` NLP syntax forces splatting for the decision variable, which implies use - of `Vararg{T,N}` (see the [performance tip](https://docs.julialang.org/en/v1/manual/performance-tips/#Be-aware-of-when-Julia-avoids-specializing)) - and memoization to avoid redundant computations. This is already complex, but it's even - worse knowing that most automatic differentiation tools do not support splatting. + of `Vararg{T,N}` (see the [performance tip][1]) and memoization to avoid redundant + computations. This is already complex, but it's even worse knowing that most automatic + differentiation tools do not support splatting. - The signature of gradient and hessian functions is not the same for univariate (`nZ̃ == 1`) and multivariate (`nZ̃ > 1`) operators in `JuMP`. Both must be defined. -Inspired from: [User-defined operators with vector outputs](https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs) +Inspired from: [User-defined operators with vector outputs][2] + +[1]: https://docs.julialang.org/en/v1/manual/performance-tips/#Be-aware-of-when-Julia-avoids-specializing +[2]: https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs """ function get_optim_functions( mpc::NonLinMPC, @@ -583,30 +600,24 @@ function get_optim_functions( ) where JNT<:Real model, transcription = mpc.estim.model, mpc.transcription # --------------------- update simulation function ------------------------------------ - function update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) - if isdifferent(Z̃arg, Z̃) - for i in eachindex(Z̃) - # Z̃cache .= Z̃arg is type unstable with Z̃arg::NTuple{N, FowardDiff.Dual} - Z̃[i] = Z̃arg[i] - end - U0 = getU0!(U0, mpc, Z̃) - ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) - Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃) - Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) - ϵ = getϵ(mpc, Z̃) - gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ) - g = con_nonlinprog!(g, mpc, model, transcription, x̂0end, Ŷ0, gc, ϵ) - geq = con_nonlinprogeq!(geq, X̂0, Û0, mpc, model, transcription, U0, Z̃) - end + function update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + U0 = getU0!(U0, mpc, Z̃) + ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) + Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃) + Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) + ϵ = getϵ(mpc, Z̃) + gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ) + g = con_nonlinprog!(g, mpc, model, transcription, x̂0end, Ŷ0, gc, ϵ) + geq = con_nonlinprogeq!(geq, X̂0, Û0, mpc, model, transcription, U0, Z̃) return nothing end - # ---------------------- JNT vectors cache -------------------------------------------- + # ----- common cache for Jfunc, gfuncs, geqfuncs called with floats ------------------- nu, ny, nx̂, nϵ, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, mpc.Hp, mpc.Hc ng, nc, neq = length(mpc.con.i_g), mpc.con.nc, mpc.con.neq nZ̃, nU, nŶ, nX̂ = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂ nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny myNaN = convert(JNT, NaN) - Z̃ = fill(myNaN, nZ̃) + Z̃ = fill(myNaN, nZ̃) # NaN to force update_simulations! at first call ΔŨ = zeros(JNT, nΔŨ) x̂0end = zeros(JNT, nx̂) Ue, Ŷe = zeros(JNT, nUe), zeros(JNT, nŶe) @@ -616,19 +627,20 @@ function get_optim_functions( geq = zeros(JNT, neq) # ---------------------- objective function ------------------------------------------ function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} - update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + if isdifferent(Z̃arg, Z̃) + Z̃ .= Z̃arg + update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + end return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T end - function Jfunc!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) - update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + function Jfunc!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) end Z̃_∇J = fill(myNaN, nZ̃) ∇J = zeros(JNT, nZ̃) # gradient of objective J ∇J_context = ( - Cache(Z̃), Cache(ΔŨ), Cache(x̂0end), - Cache(Ue), Cache(Ŷe), - Cache(U0), Cache(Ŷ0), + Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(X̂0), Cache(gc), Cache(g), Cache(geq) ) @@ -650,33 +662,29 @@ function get_optim_functions( gfuncs = Vector{Function}(undef, ng) for i in eachindex(gfuncs) gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} - update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + if isdifferent(Z̃arg, Z̃) + Z̃ .= Z̃arg + update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + end return g[i]::T end gfuncs[i] = gfunc_i end - function gfunc!(g, Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, geq) - update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) - return g + function gfunc!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, geq) + return update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) end Z̃_∇g = fill(myNaN, nZ̃) ∇g = zeros(JNT, ng, nZ̃) # Jacobian of inequality constraints g ∇g_context = ( - Cache(Z̃), Cache(ΔŨ), Cache(x̂0end), - Cache(Ue), Cache(Ŷe), - Cache(U0), Cache(Ŷ0), + Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(X̂0), Cache(gc), Cache(geq) ) - - - # TODO: cleanup the next part: + # temporarily enable all the inequality constraints for sparsity pattern detection: i_g_old = copy(mpc.con.i_g) mpc.con.i_g .= true ∇g_prep = prepare_jacobian(gfunc!, g, jac_backend, Z̃_∇g, ∇g_context...) mpc.con.i_g .= i_g_old - - ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) ∇gfuncs_i! = if nZ̃ == 1 @@ -692,7 +700,6 @@ function get_optim_functions( if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg jacobian!(gfunc!, g, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context...) - display(∇g) end return ∇g_i .= @views ∇g[i, :] # multivariate syntax, see JuMP.@operator doc end @@ -703,21 +710,21 @@ function get_optim_functions( geqfuncs = Vector{Function}(undef, neq) for i in eachindex(geqfuncs) geqfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} - update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + if isdifferent(Z̃arg, Z̃) + Z̃ .= Z̃arg + update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + end return geq[i]::T end geqfuncs[i] = geqfunc_i end - function geqfunc!(geq, Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g) - update_simulations!(Z̃arg, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) - return geq + function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g) + return update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) end Z̃_∇geq = fill(myNaN, nZ̃) ∇geq = zeros(JNT, neq, nZ̃) # Jacobian of equality constraints geq ∇geq_context = ( - Cache(Z̃), Cache(ΔŨ), Cache(x̂0end), - Cache(Ue), Cache(Ŷe), - Cache(U0), Cache(Ŷ0), + Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(X̂0), Cache(gc), Cache(g) ) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 62516b4d5..eb72a92e6 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -80,8 +80,7 @@ end Construct a steady-state Kalman Filter with the [`LinModel`](@ref) `model`. -The steady-state (or [asymptotic](https://en.wikipedia.org/wiki/Kalman_filter#Asymptotic_form)) -Kalman filter is based on the process model : +The steady-state (or [asymptotic][1]) Kalman filter is based on the process model : ```math \begin{aligned} \mathbf{x}(k+1) &= @@ -125,6 +124,8 @@ state of the next time step ``\mathbf{x̂}_k(k+1)``. This estimator is allocatio - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current estimator, in opposition to the delayed/predictor form). +[1]: https://en.wikipedia.org/wiki/Kalman_filter#Asymptotic_form + # Examples ```jldoctest julia> model = LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 0.5); @@ -160,9 +161,11 @@ SteadyKalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and: !!! tip Increasing `σQint_u` and `σQint_ym` values increases the integral action "gain". - The constructor pre-compute the steady-state Kalman gain `K̂` with the [`kalman`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/synthesis/#ControlSystemsBase.kalman-Tuple{Any,%20Any,%20Any,%20Any,%20Any,%20Vararg{Any}}) + The constructor pre-compute the steady-state Kalman gain `K̂` with the [`kalman`][2] function. It can sometimes fail, for example when `model` matrices are ill-conditioned. In such a case, you can try the alternative time-varying [`KalmanFilter`](@ref). + + [2]: https://juliacontrol.github.io/ControlSystems.jl/stable/lib/synthesis/#ControlSystemsBase.kalman-Tuple{Any,%20Any,%20Any,%20Any,%20Any,%20Vararg{Any}} """ function SteadyKalmanFilter( model::SM; diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 484156e9d..666922922 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -261,12 +261,14 @@ transcription for now. with a quadratic/nonlinear optimizer for solving (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl), or [`OSQP`](https://osqp.org/docs/parsers/jump.html) if `model` is a [`LinModel`](@ref)). - `gradient=AutoForwardDiff()` : an `AbstractADType` backend for the gradient of the objective - function if `model` is not a [`LinModel`](@ref), see [`DifferentiationInterface`](https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/explanation/backends/#List). + function if `model` is not a [`LinModel`](@ref), see [`DifferentiationInterface`][1]. - `jacobian=AutoForwardDiff()` : an `AbstractADType` backend for the Jacobian of the constraints if `model` is not a [`LinModel`](@ref), see `gradient` above for the options. - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current estimator, in opposition to the delayed/predictor form). +[1]: https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/explanation/backends/#List + # Examples ```jldoctest julia> model = NonLinModel((x,u,_,_)->0.1x+u, (x,_,_)->2x, 10.0, 1, 1, 1, solver=nothing); @@ -346,9 +348,11 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s, Ipopt optimizer default, a [`KalmanFilter`](@ref) estimates the arrival covariance (customizable). - Else, a nonlinear program with automatic differentiation (AD) solves the optimization. Optimizers generally benefit from exact derivatives like AD. However, the `f` and `h` - functions must be compatible with this feature. See [Automatic differentiation](https://jump.dev/JuMP.jl/stable/manual/nlp/#Automatic-differentiation) + functions must be compatible with this feature. See the `JuMP` [documentation][3] for common mistakes when writing these functions. An [`UnscentedKalmanFilter`](@ref) estimates the arrival covariance by default. + + [3]: https://jump.dev/JuMP.jl/stable/manual/nonlinear/#Common-mistakes-when-writing-a-user-defined-operator The slack variable ``ϵ`` relaxes the constraints if enabled, see [`setconstraint!`](@ref). It is disabled by default for the MHE (from `Cwt=Inf`) but it should be activated for diff --git a/src/model/linearization.jl b/src/model/linearization.jl index efcb64b5e..a070cf4d2 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -131,8 +131,10 @@ julia> linmodel.A equations are similar if the nonlinear model has nonzero operating points. Automatic differentiation (AD) allows exact Jacobians. The [`NonLinModel`](@ref) `f` and - `h` functions must be compatible with this feature though. See [Automatic differentiation](https://jump.dev/JuMP.jl/stable/manual/nlp/#Automatic-differentiation) + `h` functions must be compatible with this feature though. See `JuMP` [documentation][3] for common mistakes when writing these functions. + + [3]: https://jump.dev/JuMP.jl/stable/manual/nonlinear/#Common-mistakes-when-writing-a-user-defined-operator """ function linearize(model::SimModel{NT}; kwargs...) where NT<:Real nu, nx, ny, nd = model.nu, model.nx, model.ny, model.nd diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index 24f40ba05..608468384 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -112,12 +112,12 @@ LinModel with a sample time Ts = 0.1 s and: \mathbf{y}(k) &= \mathbf{C x}(k) + \mathbf{D z}(k) \end{aligned} ``` - Continuous dynamics are internally discretized using [`c2d`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.c2d) - and `:zoh` for manipulated inputs, and `:tustin`, for measured disturbances. Lastly, if - `sys` is discrete and the provided argument `Ts ≠ sys.Ts`, the system is resampled by - using the aforementioned discretization methods. + Continuous dynamics are internally discretized using [`c2d`][1] and `:zoh` for + manipulated inputs, and `:tustin`, for measured disturbances. Lastly, if `sys` is + discrete and the provided argument `Ts ≠ sys.Ts`, the system is resampled by using the + aforementioned discretization methods. - Note that the constructor transforms the system to its minimal realization using [`minreal`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.minreal) + Note that the constructor transforms the system to its minimal realization using [`minreal`][2] for controllability/observability. As a consequence, the final state-space representation may be different from the one provided in `sys`. It is also converted into a more practical form (``\mathbf{D_u=0}`` because of the zero-order hold): @@ -129,6 +129,9 @@ LinModel with a sample time Ts = 0.1 s and: ``` Use the syntax [`LinModel{NT}(A, Bu, C, Bd, Dd, Ts)`](@ref) to force a specific state-space representation. + + [1]: https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.c2d + [2]: https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.minreal """ function LinModel( sys::StateSpace{E, NT}, diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 96dd64b6b..46c853730 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -95,10 +95,13 @@ form. The optional parameter `NT` explicitly set the number type of vectors (def !!! warning The two functions must be in pure Julia to use the model in [`NonLinMPC`](@ref), - [`ExtendedKalmanFilter`](@ref), [`MovingHorizonEstimator`](@ref) and [`linearize`](@ref). + [`ExtendedKalmanFilter`](@ref), [`MovingHorizonEstimator`](@ref) and [`linearize`](@ref), + except if a finite difference backend is used (e.g. [`AutoFiniteDiff`][1]). See also [`LinModel`](@ref). +[1]: https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/explanation/backends/#List + # Examples ```jldoctest julia> f!(ẋ, x, u, _ , p) = (ẋ .= p*x .+ u; nothing); From 9ce0a56df1714d020568ee1b380971f4d5205d7d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 17 Mar 2025 13:49:33 -0400 Subject: [PATCH 08/30] docs: making use of `DocumenterInterLink` --- docs/Project.toml | 13 +++++++------ docs/make.jl | 10 +++++++++- src/controller/linmpc.jl | 4 ++-- src/controller/nonlinmpc.jl | 25 ++++++++----------------- src/estimator/kalman.jl | 9 +++------ src/estimator/luenberger.jl | 4 ++-- src/estimator/mhe/construct.jl | 12 ++++-------- src/model/linearization.jl | 4 +--- src/model/linmodel.jl | 17 +++++++---------- 9 files changed, 43 insertions(+), 55 deletions(-) diff --git a/docs/Project.toml b/docs/Project.toml index cd71da677..1be7cf0fe 100644 --- a/docs/Project.toml +++ b/docs/Project.toml @@ -1,19 +1,20 @@ [deps] -LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" -Logging = "56ddb016-857b-54e1-b83d-db4d58db5568" ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" DAQP = "c47d62df-3981-49c8-9651-128b1cd08617" Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4" +DocumenterInterLinks = "d12716ef-a0f6-4df4-a9f1-a5a34e75c656" JuMP = "4076af6c-e467-56ae-b986-b466b2749572" +LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +Logging = "56ddb016-857b-54e1-b83d-db4d58db5568" ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78" Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" [compat] -LinearAlgebra = "1.10" -Logging = "1.10" ControlSystemsBase = "1" +DAQP = "0.6" Documenter = "1" JuMP = "1" -DAQP = "0.6" -Plots = "1" +LinearAlgebra = "1.10" +Logging = "1.10" ModelingToolkit = "9.50" +Plots = "1" diff --git a/docs/make.jl b/docs/make.jl index 37de69d4b..49f7993d3 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -3,9 +3,16 @@ ENV["PLOTS_TEST"] = "true" ENV["GKSwstype"] = "nul" push!(LOAD_PATH,"../src/") -using Documenter +using Documenter, DocumenterInterLinks using ModelPredictiveControl +links = InterLinks( + "Julia" => "https://docs.julialang.org/en/v1/objects.inv", + "ControlSystemsBase" => "https://juliacontrol.github.io/ControlSystems.jl/stable/objects.inv", + "JuMP" => "https://jump.dev/JuMP.jl/stable/objects.inv", + "DifferentiationInterface" => "https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/objects.inv", +) + DocMeta.setdocmeta!( ModelPredictiveControl, :DocTestSetup, @@ -16,6 +23,7 @@ makedocs( sitename = "ModelPredictiveControl.jl", #format = Documenter.LaTeX(platform = "none"), doctest = true, + plugins = [links], format = Documenter.HTML( prettyurls = get(ENV, "CI", nothing) == "true", edit_link = "main" diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 5ce357404..8ebeca159 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -146,8 +146,8 @@ arguments. This controller allocates memory at each time step for the optimizati - `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only. - `transcription=SingleShooting()` : a [`TranscriptionMethod`](@ref) for the optimization. - `optim=JuMP.Model(OSQP.MathOptInterfaceOSQP.Optimizer)` : quadratic optimizer used in - the predictive controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model) - (default to [`OSQP`](https://osqp.org/docs/parsers/jump.html) optimizer). + the predictive controller, provided as a [`JuMP.Model`](@extref) object (default to + [`OSQP`](https://osqp.org/docs/parsers/jump.html) optimizer). - additional keyword arguments are passed to [`SteadyKalmanFilter`](@ref) constructor. # Examples diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 518c20106..56a6afb93 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -194,17 +194,14 @@ This controller allocates memory at each time step for the optimization. - `p=model.p` : ``J_E`` and ``\mathbf{g_c}`` functions parameter ``\mathbf{p}`` (any type). - `transcription=SingleShooting()` : a [`TranscriptionMethod`](@ref) for the optimization. - `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the predictive - controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model) - (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl) optimizer). + controller, provided as a [`JuMP.Model`](@extref) object (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl) optimizer). - `gradient=AutoForwardDiff()` : an `AbstractADType` backend for the gradient of the objective - function, see [`DifferentiationInterface`][1]. + function, see [`DifferentiationInterface`](@extref DifferentiationInterface List). - `jacobian=default_jacobian(transcription)` : an `AbstractADType` backend for the Jacobian of the nonlinear constraints, see `gradient` above for the options (default in Extended Help). - additional keyword arguments are passed to [`UnscentedKalmanFilter`](@ref) constructor (or [`SteadyKalmanFilter`](@ref), for [`LinModel`](@ref)). -[1]: https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/explanation/backends/#List - # Examples ```jldoctest julia> model = NonLinModel((x,u,_,_)->0.5x+u, (x,_,_)->2x, 10.0, 1, 1, 1, solver=nothing); @@ -253,7 +250,7 @@ NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedK By default, the optimization relies on dense [`ForwardDiff`](https://github.com/JuliaDiff/ForwardDiff.jl) automatic differentiation (AD) to compute the objective and constraint derivatives. One exception: if `transcription` is not a [`SingleShooting`](@ref), the `jacobian` argument - defaults to this [sparse backend][2]: + defaults to this [sparse backend](@extref DifferentiationInterface Sparsity): ```julia AutoSparse( AutoForwardDiff(); @@ -262,14 +259,11 @@ NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedK ) ``` Optimizers generally benefit from exact derivatives like AD. However, the [`NonLinModel`](@ref) - state-space functions must be compatible with this feature. See `JuMP` [documentation][3] + state-space functions must be compatible with this feature. See [`JuMP` documentation](@extref JuMP Common-mistakes-when-writing-a-user-defined-operator) for common mistakes when writing these functions. Note that if `Cwt≠Inf`, the attribute `nlp_scaling_max_gradient` of `Ipopt` is set to `10/Cwt` (if not already set), to scale the small values of ``ϵ``. - - [2]: https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/explanation/advanced/#Sparsity - [3]: https://jump.dev/JuMP.jl/stable/manual/nonlinear/#Common-mistakes-when-writing-a-user-defined-operator """ function NonLinMPC( model::SimModel; @@ -581,16 +575,13 @@ This method is really intricate and I'm not proud of it. That's because of 3 ele - These functions are used inside the nonlinear optimization, so they must be type-stable and as efficient as possible. - The `JuMP` NLP syntax forces splatting for the decision variable, which implies use - of `Vararg{T,N}` (see the [performance tip][1]) and memoization to avoid redundant - computations. This is already complex, but it's even worse knowing that most automatic - differentiation tools do not support splatting. + of `Vararg{T,N}` (see the [performance tip][@extref Julia Be-aware-of-when-Julia-avoids-specializing] + ) and memoization to avoid redundant computations. This is already complex, but it's even + worse knowing that most automatic differentiation tools do not support splatting. - The signature of gradient and hessian functions is not the same for univariate (`nZ̃ == 1`) and multivariate (`nZ̃ > 1`) operators in `JuMP`. Both must be defined. -Inspired from: [User-defined operators with vector outputs][2] - -[1]: https://docs.julialang.org/en/v1/manual/performance-tips/#Be-aware-of-when-Julia-avoids-specializing -[2]: https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs +Inspired from: [User-defined operators with vector outputs](@extref JuMP User-defined-operators-with-vector-outputs) """ function get_optim_functions( mpc::NonLinMPC, diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index eb72a92e6..3ff44d4ba 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -80,7 +80,8 @@ end Construct a steady-state Kalman Filter with the [`LinModel`](@ref) `model`. -The steady-state (or [asymptotic][1]) Kalman filter is based on the process model : +The steady-state (or [asymptotic](https://en.wikipedia.org/wiki/Kalman_filter#Asymptotic_form)) +Kalman filter is based on the process model: ```math \begin{aligned} \mathbf{x}(k+1) &= @@ -124,8 +125,6 @@ state of the next time step ``\mathbf{x̂}_k(k+1)``. This estimator is allocatio - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current estimator, in opposition to the delayed/predictor form). -[1]: https://en.wikipedia.org/wiki/Kalman_filter#Asymptotic_form - # Examples ```jldoctest julia> model = LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 0.5); @@ -161,11 +160,9 @@ SteadyKalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and: !!! tip Increasing `σQint_u` and `σQint_ym` values increases the integral action "gain". - The constructor pre-compute the steady-state Kalman gain `K̂` with the [`kalman`][2] + The constructor pre-compute the steady-state Kalman gain `K̂` with the [`kalman`](@extref ControlSystemsBase.kalman) function. It can sometimes fail, for example when `model` matrices are ill-conditioned. In such a case, you can try the alternative time-varying [`KalmanFilter`](@ref). - - [2]: https://juliacontrol.github.io/ControlSystems.jl/stable/lib/synthesis/#ControlSystemsBase.kalman-Tuple{Any,%20Any,%20Any,%20Any,%20Any,%20Vararg{Any}} """ function SteadyKalmanFilter( model::SM; diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 6000cf073..26f841e6e 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -77,8 +77,8 @@ Extended Help). The argument `poles` is a vector of `model.nx + sum(nint_u) + su elements specifying the observer poles/eigenvalues (near ``z=0.5`` by default). The observer is constructed with a direct transmission from ``\mathbf{y^m}`` if `direct=true` (a.k.a. current observers, in opposition to the delayed/prediction form). The method computes the -observer gain `K̂` with [`place`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/synthesis/#ControlSystemsBase.place). -This estimator is allocation-free. +observer gain `K̂` with [`place`](@extref ControlSystemsBase.place) function. This estimator +is allocation-free. # Examples ```jldoctest diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 666922922..72b229288 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -257,18 +257,16 @@ transcription for now. - `σPint_ym_0=fill(1,sum(nint_ym))` or *`sigmaPint_ym_0`* : same than `σP_0` but for the unmeasured disturbances at measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators). - `Cwt=Inf` : slack variable weight ``C``, default to `Inf` meaning hard constraints only. -- `optim=default_optim_mhe(model)` : a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model) - with a quadratic/nonlinear optimizer for solving (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl), +- `optim=default_optim_mhe(model)` : a [`JuMP.Model`](@extref) object with a quadratic or + nonlinear optimizer for solving (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl), or [`OSQP`](https://osqp.org/docs/parsers/jump.html) if `model` is a [`LinModel`](@ref)). - `gradient=AutoForwardDiff()` : an `AbstractADType` backend for the gradient of the objective - function if `model` is not a [`LinModel`](@ref), see [`DifferentiationInterface`][1]. + function if `model` is not a [`LinModel`](@ref), see [`DifferentiationInterface`][@extref DifferentiationInterface List]. - `jacobian=AutoForwardDiff()` : an `AbstractADType` backend for the Jacobian of the constraints if `model` is not a [`LinModel`](@ref), see `gradient` above for the options. - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current estimator, in opposition to the delayed/predictor form). -[1]: https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/explanation/backends/#List - # Examples ```jldoctest julia> model = NonLinModel((x,u,_,_)->0.1x+u, (x,_,_)->2x, 10.0, 1, 1, 1, solver=nothing); @@ -348,11 +346,9 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s, Ipopt optimizer default, a [`KalmanFilter`](@ref) estimates the arrival covariance (customizable). - Else, a nonlinear program with automatic differentiation (AD) solves the optimization. Optimizers generally benefit from exact derivatives like AD. However, the `f` and `h` - functions must be compatible with this feature. See the `JuMP` [documentation][3] + functions must be compatible with this feature. See the [`JuMP` documentation](@extref JuMP Common-mistakes-when-writing-a-user-defined-operator) for common mistakes when writing these functions. An [`UnscentedKalmanFilter`](@ref) estimates the arrival covariance by default. - - [3]: https://jump.dev/JuMP.jl/stable/manual/nonlinear/#Common-mistakes-when-writing-a-user-defined-operator The slack variable ``ϵ`` relaxes the constraints if enabled, see [`setconstraint!`](@ref). It is disabled by default for the MHE (from `Cwt=Inf`) but it should be activated for diff --git a/src/model/linearization.jl b/src/model/linearization.jl index a070cf4d2..d02fdda91 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -131,10 +131,8 @@ julia> linmodel.A equations are similar if the nonlinear model has nonzero operating points. Automatic differentiation (AD) allows exact Jacobians. The [`NonLinModel`](@ref) `f` and - `h` functions must be compatible with this feature though. See `JuMP` [documentation][3] + `h` functions must be compatible with this feature though. See [`JuMP` documentation][@extref JuMP Common-mistakes-when-writing-a-user-defined-operator] for common mistakes when writing these functions. - - [3]: https://jump.dev/JuMP.jl/stable/manual/nonlinear/#Common-mistakes-when-writing-a-user-defined-operator """ function linearize(model::SimModel{NT}; kwargs...) where NT<:Real nu, nx, ny, nd = model.nu, model.nx, model.ny, model.nd diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index 608468384..041ebeea4 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -84,7 +84,7 @@ resamples discrete ones if `Ts ≠ sys.Ts`, computes a new realization if not mi separates the ``\mathbf{z}`` terms in two parts (details in Extended Help). The rest of the documentation assumes discrete dynamics since all systems end up in this form. -See also [`ss`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.ss) +See also [`ss`](@extref ControlSystemsBase.ss) # Examples ```jldoctest @@ -112,12 +112,12 @@ LinModel with a sample time Ts = 0.1 s and: \mathbf{y}(k) &= \mathbf{C x}(k) + \mathbf{D z}(k) \end{aligned} ``` - Continuous dynamics are internally discretized using [`c2d`][1] and `:zoh` for - manipulated inputs, and `:tustin`, for measured disturbances. Lastly, if `sys` is - discrete and the provided argument `Ts ≠ sys.Ts`, the system is resampled by using the - aforementioned discretization methods. + Continuous dynamics are internally discretized using [`c2d`](@extref ControlSystemsBase.c2d) + and `:zoh` for manipulated inputs, and `:tustin`, for measured disturbances. Lastly, if + `sys` is discrete and the provided argument `Ts ≠ sys.Ts`, the system is resampled by + using the aforementioned discretization methods. - Note that the constructor transforms the system to its minimal realization using [`minreal`][2] + Note that the constructor transforms the system to its minimal realization using [`minreal`](@extref ControlSystemsBase.minreal) for controllability/observability. As a consequence, the final state-space representation may be different from the one provided in `sys`. It is also converted into a more practical form (``\mathbf{D_u=0}`` because of the zero-order hold): @@ -129,9 +129,6 @@ LinModel with a sample time Ts = 0.1 s and: ``` Use the syntax [`LinModel{NT}(A, Bu, C, Bd, Dd, Ts)`](@ref) to force a specific state-space representation. - - [1]: https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.c2d - [2]: https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.minreal """ function LinModel( sys::StateSpace{E, NT}, @@ -195,7 +192,7 @@ Convert to minimal realization state-space when `sys` is a transfer function. `sys` is equal to ``\frac{\mathbf{y}(s)}{\mathbf{z}(s)}`` for continuous-time, and ``\frac{\mathbf{y}(z)}{\mathbf{z}(z)}``, for discrete-time. -See also [`tf`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.tf) +See also [`tf`](@extref ControlSystemsBase.tf) # Examples ```jldoctest From 22a6b9cacd260905fa1f4612f98717ebf0f0f1df Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 17 Mar 2025 14:39:59 -0400 Subject: [PATCH 09/30] doc: clean up in the inter-links --- docs/make.jl | 1 + docs/src/manual/nonlinmpc.md | 2 +- src/controller/nonlinmpc.jl | 16 ++++++++-------- src/estimator/kalman.jl | 10 +++++----- src/estimator/mhe/construct.jl | 2 +- src/model/linearization.jl | 4 ++-- src/model/nonlinmodel.jl | 4 +--- 7 files changed, 19 insertions(+), 20 deletions(-) diff --git a/docs/make.jl b/docs/make.jl index 49f7993d3..de69e08d8 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -11,6 +11,7 @@ links = InterLinks( "ControlSystemsBase" => "https://juliacontrol.github.io/ControlSystems.jl/stable/objects.inv", "JuMP" => "https://jump.dev/JuMP.jl/stable/objects.inv", "DifferentiationInterface" => "https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/objects.inv", + "ForwardDiff" => "https://juliadiff.org/ForwardDiff.jl/stable/objects.inv", ) DocMeta.setdocmeta!( diff --git a/docs/src/manual/nonlinmpc.md b/docs/src/manual/nonlinmpc.md index 57bc2f28d..1c5032b3b 100644 --- a/docs/src/manual/nonlinmpc.md +++ b/docs/src/manual/nonlinmpc.md @@ -329,7 +329,7 @@ Nonlinear MPC is more computationally expensive than [`LinMPC`](@ref). Solving t should always be faster than the sampling time ``T_s = 0.1`` s for real-time operation. This requirement is sometimes hard to meet on electronics or mechanical systems because of the fast dynamics. To ease the design and comparison with [`LinMPC`](@ref), the [`linearize`](@ref) -function allows automatic linearization of [`NonLinModel`](@ref) based on [`ForwardDiff.jl`](https://juliadiff.org/ForwardDiff.jl/stable/). +function allows automatic linearization of [`NonLinModel`](@ref) based on [`ForwardDiff`](@extref ForwardDiff). We first linearize `model` at the point ``θ = π`` rad and ``ω = τ = 0`` (inverted position): ```@example man_nonlin diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 56a6afb93..ef406d8f7 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -196,7 +196,7 @@ This controller allocates memory at each time step for the optimization. - `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the predictive controller, provided as a [`JuMP.Model`](@extref) object (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl) optimizer). - `gradient=AutoForwardDiff()` : an `AbstractADType` backend for the gradient of the objective - function, see [`DifferentiationInterface`](@extref DifferentiationInterface List). + function, see [`DifferentiationInterface` doc](@extref DifferentiationInterface List). - `jacobian=default_jacobian(transcription)` : an `AbstractADType` backend for the Jacobian of the nonlinear constraints, see `gradient` above for the options (default in Extended Help). - additional keyword arguments are passed to [`UnscentedKalmanFilter`](@ref) constructor @@ -247,16 +247,16 @@ NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedK The keyword argument `nc` is the number of elements in `LHS`, and `gc!`, an alias for the `gc` argument (both `gc` and `gc!` accepts non-mutating and mutating functions). - By default, the optimization relies on dense [`ForwardDiff`](https://github.com/JuliaDiff/ForwardDiff.jl) + By default, the optimization relies on dense [`ForwardDiff`](@extref ForwardDiff) automatic differentiation (AD) to compute the objective and constraint derivatives. One exception: if `transcription` is not a [`SingleShooting`](@ref), the `jacobian` argument - defaults to this [sparse backend](@extref DifferentiationInterface Sparsity): + defaults to this [sparse backend](@extref DifferentiationInterface AutoSparse-object): ```julia - AutoSparse( - AutoForwardDiff(); - sparsity_detector = TracerSparsityDetector(), - coloring_algorithm = GreedyColoringAlgorithm() - ) + AutoSparse( + AutoForwardDiff(); + sparsity_detector = TracerSparsityDetector(), + coloring_algorithm = GreedyColoringAlgorithm() + ) ``` Optimizers generally benefit from exact derivatives like AD. However, the [`NonLinModel`](@ref) state-space functions must be compatible with this feature. See [`JuMP` documentation](@extref JuMP Common-mistakes-when-writing-a-user-defined-operator) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 3ff44d4ba..44496422b 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -951,8 +951,8 @@ Construct an extended Kalman Filter with the [`SimModel`](@ref) `model`. Both [`LinModel`](@ref) and [`NonLinModel`](@ref) are supported. The process model and the keyword arguments are identical to [`UnscentedKalmanFilter`](@ref), except for `α`, `β` and `κ` which do not apply to the extended Kalman Filter. The Jacobians of the augmented model -``\mathbf{f̂, ĥ}`` are computed with [`ForwardDiff.jl`](https://github.com/JuliaDiff/ForwardDiff.jl) -automatic differentiation. This estimator allocates memory for the Jacobians. +``\mathbf{f̂, ĥ}`` are computed with [`ForwardDiff`](@extref ForwardDiff) automatic +differentiation. This estimator allocates memory for the Jacobians. !!! warning See the Extended Help of [`linearize`](@ref) function if you get an error like: @@ -1043,9 +1043,9 @@ augmented process model: \end{aligned} ``` The matrix ``\mathbf{Ĥ^m}`` is the rows of ``\mathbf{Ĥ}`` that are measured outputs. The -function [`ForwardDiff.jacobian`](https://juliadiff.org/ForwardDiff.jl/stable/user/api/#ForwardDiff.jacobian) -automatically computes them. The correction and prediction step equations are provided below. -The correction step is skipped if `estim.direct == true` since it's already done by the user. +Jacobians are computed with [`ForwardDiff`](@extref ForwardDiff). The correction and +prediction step equations are provided below. The correction step is skipped if +`estim.direct == true` since it's already done by the user. # Correction Step ```math diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 72b229288..f5f4d6eeb 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -261,7 +261,7 @@ transcription for now. nonlinear optimizer for solving (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl), or [`OSQP`](https://osqp.org/docs/parsers/jump.html) if `model` is a [`LinModel`](@ref)). - `gradient=AutoForwardDiff()` : an `AbstractADType` backend for the gradient of the objective - function if `model` is not a [`LinModel`](@ref), see [`DifferentiationInterface`][@extref DifferentiationInterface List]. + function if `model` is not a [`LinModel`](@ref), see [`DifferentiationInterface` doc](@extref DifferentiationInterface List). - `jacobian=AutoForwardDiff()` : an `AbstractADType` backend for the Jacobian of the constraints if `model` is not a [`LinModel`](@ref), see `gradient` above for the options. - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current diff --git a/src/model/linearization.jl b/src/model/linearization.jl index d02fdda91..65626caf2 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -75,7 +75,7 @@ Linearize `model` at the operating points `x`, `u`, `d` and return the [`LinMode The arguments `x`, `u` and `d` are the linearization points for the state ``\mathbf{x}``, manipulated input ``\mathbf{u}`` and measured disturbance ``\mathbf{d}``, respectively (not necessarily an equilibrium, details in Extended Help). The Jacobians of ``\mathbf{f}`` and -``\mathbf{h}`` functions are automatically computed with [`ForwardDiff.jl`](https://github.com/JuliaDiff/ForwardDiff.jl). +``\mathbf{h}`` functions are automatically computed with [`ForwardDiff`](@extref ForwardDiff). !!! warning See Extended Help if you get an error like: @@ -131,7 +131,7 @@ julia> linmodel.A equations are similar if the nonlinear model has nonzero operating points. Automatic differentiation (AD) allows exact Jacobians. The [`NonLinModel`](@ref) `f` and - `h` functions must be compatible with this feature though. See [`JuMP` documentation][@extref JuMP Common-mistakes-when-writing-a-user-defined-operator] + `h` functions must be compatible with this feature though. See [`JuMP` documentation](@extref JuMP Common-mistakes-when-writing-a-user-defined-operator) for common mistakes when writing these functions. """ function linearize(model::SimModel{NT}; kwargs...) where NT<:Real diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 46c853730..b5676e822 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -96,12 +96,10 @@ form. The optional parameter `NT` explicitly set the number type of vectors (def !!! warning The two functions must be in pure Julia to use the model in [`NonLinMPC`](@ref), [`ExtendedKalmanFilter`](@ref), [`MovingHorizonEstimator`](@ref) and [`linearize`](@ref), - except if a finite difference backend is used (e.g. [`AutoFiniteDiff`][1]). + except if a finite difference backend is used (e.g. [`AutoFiniteDiff`](@extref DifferentiationInterface List). See also [`LinModel`](@ref). -[1]: https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/explanation/backends/#List - # Examples ```jldoctest julia> f!(ẋ, x, u, _ , p) = (ẋ .= p*x .+ u; nothing); From 1ed36a8949d2aa8636777c821adb34f1d7a3cfac Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 17 Mar 2025 16:02:04 -0400 Subject: [PATCH 10/30] added: use `DI.Cache` in `MovingHorizonEstimator` --- src/controller/nonlinmpc.jl | 3 + src/estimator/mhe/construct.jl | 101 ++++++++++++++++----------------- src/estimator/mhe/execute.jl | 9 +++ 3 files changed, 61 insertions(+), 52 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index ef406d8f7..1f7227790 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -590,6 +590,9 @@ function get_optim_functions( jac_backend ::AbstractADType ) where JNT<:Real model, transcription = mpc.estim.model, mpc.transcription + #TODO: initialize jacobian as sparsed if it's the case? + #TODO: fix type of all cache to ::Vector{JNT} (verify performance difference with and w/o) + #TODO: mêmes choses pour le MHE # --------------------- update simulation function ------------------------------------ function update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) U0 = getU0!(U0, mpc, Z̃) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index f5f4d6eeb..f01467e04 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1333,92 +1333,89 @@ function get_optim_functions( jac_backend::AbstractADType ) where {JNT <: Real} model, con = estim.model, estim.con - nx̂, nym, nŷ, nu, nϵ, He = estim.nx̂, estim.nym, model.ny, model.nu, estim.nϵ, estim.He - nV̂, nX̂, ng, nZ̃ = He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) - Ncache = nZ̃ + 3 - myNaN = convert(JNT, NaN) # fill Z̃ with NaNs to force update_simulations! at 1st call: - # ---------------------- differentiation cache --------------------------------------- - Z̃_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(fill(myNaN, nZ̃), Ncache) - V̂_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nV̂), Ncache) - g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Ncache) - X̂0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nX̂), Ncache) - x̄_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Ncache) - û0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nu), Ncache) - ŷ0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŷ), Ncache) # --------------------- update simulation function ------------------------------------ - function update_simulations!( - Z̃arg::Union{NTuple{N, T}, AbstractVector{T}}, Z̃cache - ) where {N, T <:Real} - if isdifferent(Z̃cache, Z̃arg) - for i in eachindex(Z̃cache) - # Z̃cache .= Z̃arg is type unstable with Z̃arg::NTuple{N, FowardDiff.Dual} - Z̃cache[i] = Z̃arg[i] - end - Z̃ = Z̃cache - ϵ = (nϵ ≠ 0) ? Z̃[begin] : zero(T) # ϵ = 0 if Cwt=Inf (meaning: no relaxation) - V̂, X̂0 = get_tmp(V̂_cache, T), get_tmp(X̂0_cache, T) - û0, ŷ0 = get_tmp(û0_cache, T), get_tmp(ŷ0_cache, T) - g = get_tmp(g_cache, T) - V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, Z̃) - g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ) - end + function update_simulations!(Z̃, V̂, X̂0, û0, ŷ0, g) + V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, Z̃) + ϵ = getϵ(estim, Z̃) + g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ) return nothing end + # ---------- common cache for Jfunc, gfuncs called with floats ------------------------ + nx̂, nym, nŷ, nu, nϵ, He = estim.nx̂, estim.nym, model.ny, model.nu, estim.nϵ, estim.He + nV̂, nX̂, ng, nZ̃ = He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) + myNaN = convert(JNT, NaN) + Z̃ = fill(myNaN, nZ̃) # NaN to force update_simulations! at first call + V̂, X̂0 = zeros(JNT, nV̂), zeros(JNT, nX̂) + û0, ŷ0 = zeros(JNT, nu), zeros(JNT, nŷ) + g = zeros(JNT, ng) + x̄ = zeros(JNT, nx̂) # --------------------- objective functions ------------------------------------------- function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} - Z̃ = get_tmp(Z̃_cache, T) - update_simulations!(Z̃arg, Z̃) - x̄, V̂ = get_tmp(x̄_cache, T), get_tmp(V̂_cache, T) + if isdifferent(Z̃arg, Z̃) + Z̃ .= Z̃arg + update_simulations!(Z̃, V̂, X̂0, û0, ŷ0, g) + end return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T end - function Jfunc_vec(Z̃arg::AbstractVector{T}) where T<:Real - Z̃ = get_tmp(Z̃_cache, T) - update_simulations!(Z̃arg, Z̃) - x̄, V̂ = get_tmp(x̄_cache, T), get_tmp(V̂_cache, T) - return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T + function Jfunc!(Z̃, V̂, X̂0, û0, ŷ0, g, x̄) + update_simulations!(Z̃, V̂, X̂0, û0, ŷ0, g) + return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) end Z̃_∇J = fill(myNaN, nZ̃) ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J - ∇J_prep = prepare_gradient(Jfunc_vec, grad_backend, Z̃_∇J) + ∇J_context = ( + Cache(V̂), Cache(X̂0), + Cache(û0), Cache(ŷ0), + Cache(g), + Cache(x̄), + ) + ∇J_prep = prepare_gradient(Jfunc!, grad_backend, Z̃_∇J, ∇J_context...) ∇Jfunc! = if nZ̃ == 1 - function (Z̃arg::T) where T<:Real + function (Z̃arg) Z̃_∇J .= Z̃arg - gradient!(Jfunc_vec, ∇J, ∇J_prep, grad_backend, Z̃_∇J) + gradient!(Jfunc!, ∇J, ∇J_prep, grad_backend, Z̃_∇J, ∇J_context...) return ∇J[begin] # univariate syntax, see JuMP.@operator doc end else function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} Z̃_∇J .= Z̃arg - gradient!(Jfunc_vec, ∇J, ∇J_prep, grad_backend, Z̃_∇J) + gradient!(Jfunc!, ∇J, ∇J_prep, grad_backend, Z̃_∇J, ∇J_context...) return ∇J # multivariate syntax, see JuMP.@operator doc end end # --------------------- inequality constraint functions ------------------------------- gfuncs = Vector{Function}(undef, ng) for i in eachindex(gfuncs) - func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} - update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) - g = get_tmp(g_cache, T) + gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} + if isdifferent(Z̃arg, Z̃) + Z̃ .= Z̃arg + update_simulations!(Z̃, V̂, X̂0, û0, ŷ0, g) + end return g[i]::T end - gfuncs[i] = func_i + gfuncs[i] = gfunc_i end - function gfunc_vec!(g, Z̃vec::AbstractVector{T}) where T<:Real - update_simulations!(Z̃vec, get_tmp(Z̃_cache, T)) - g .= get_tmp(g_cache, T) - return g + function gfunc!(g, Z̃, V̂, X̂0, û0, ŷ0) + return update_simulations!(Z̃, V̂, X̂0, û0, ŷ0, g) end Z̃_∇g = fill(myNaN, nZ̃) - g_vec = Vector{JNT}(undef, ng) ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of inequality constraints g - ∇g_prep = prepare_jacobian(gfunc_vec!, g_vec, jac_backend, Z̃_∇g) + ∇g_context = ( + Cache(V̂), Cache(X̂0), + Cache(û0), Cache(ŷ0), + ) + # temporarily enable all the inequality constraints for sparsity pattern detection: + i_g_old = copy(estim.con.i_g) + estim.con.i_g .= true + ∇g_prep = prepare_jacobian(gfunc!, g, jac_backend, Z̃_∇g, ∇g_context...) + estim.con.i_g .= i_g_old ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) ∇gfuncs![i] = if nZ̃ == 1 function (Z̃arg::T) where T<:Real if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!(gfunc_vec!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g) + jacobian!(gfunc!, g, ∇g, ∇g_prep, jac_backend, Z̃_∇g. ∇g_context...) end return ∇g[i, begin] # univariate syntax, see JuMP.@operator doc end @@ -1426,7 +1423,7 @@ function get_optim_functions( function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!(gfunc_vec!, g_vec, ∇g, ∇g_prep, jac_backend, Z̃_∇g) + jacobian!(gfunc!, g, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context...) end return ∇g_i .= @views ∇g[i, :] # multivariate syntax, see JuMP.@operator doc end diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 0511eba81..1afbfdc76 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -160,6 +160,15 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real return info end +""" + getϵ(estim::MovingHorizonEstimator, Z̃) -> ϵ + +Get the slack `ϵ` from the decision vector `Z̃` if present, otherwise return 0. +""" +function getϵ(estim::MovingHorizonEstimator, Z̃::AbstractVector{NT}) where NT<:Real + return estim.nϵ ≠ 0 ? Z̃[begin] : zero(NT) +end + """ add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.lastu0) -> ismoving From c713f61731af1a94b3b8f71dc442a33cc96751e5 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 17 Mar 2025 16:54:29 -0400 Subject: [PATCH 11/30] added: init diff. matrix as dense or sparse as required by backend --- src/ModelPredictiveControl.jl | 2 +- src/controller/nonlinmpc.jl | 9 ++++----- src/estimator/mhe/construct.jl | 4 ++-- src/general.jl | 4 ++++ 4 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index c7b844a04..9d7d699d0 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -11,7 +11,7 @@ using DifferentiationInterface: ADTypes.AbstractADType, AutoForwardDiff, AutoSpa using DifferentiationInterface: gradient!, jacobian!, prepare_gradient, prepare_jacobian using DifferentiationInterface: Constant, Cache using SparseConnectivityTracer: TracerSparsityDetector -using SparseMatrixColorings: GreedyColoringAlgorithm +using SparseMatrixColorings: GreedyColoringAlgorithm, sparsity_pattern import ForwardDiff #TODO: delete this after `linearize!` and `ExtendedKalmanFilter` are updated diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 1f7227790..6e6f8141b 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -590,7 +590,6 @@ function get_optim_functions( jac_backend ::AbstractADType ) where JNT<:Real model, transcription = mpc.estim.model, mpc.transcription - #TODO: initialize jacobian as sparsed if it's the case? #TODO: fix type of all cache to ::Vector{JNT} (verify performance difference with and w/o) #TODO: mêmes choses pour le MHE # --------------------- update simulation function ------------------------------------ @@ -631,14 +630,14 @@ function get_optim_functions( update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) end - Z̃_∇J = fill(myNaN, nZ̃) - ∇J = zeros(JNT, nZ̃) # gradient of objective J + Z̃_∇J = fill(myNaN, nZ̃) ∇J_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(X̂0), Cache(gc), Cache(g), Cache(geq) ) ∇J_prep = prepare_gradient(Jfunc!, grad_backend, Z̃_∇J, ∇J_context...) + ∇J = Vector{JNT}(undef, nZ̃) ∇Jfunc! = if nZ̃ == 1 function (Z̃arg) Z̃_∇J .= Z̃arg @@ -668,7 +667,6 @@ function get_optim_functions( return update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) end Z̃_∇g = fill(myNaN, nZ̃) - ∇g = zeros(JNT, ng, nZ̃) # Jacobian of inequality constraints g ∇g_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(X̂0), @@ -679,6 +677,7 @@ function get_optim_functions( mpc.con.i_g .= true ∇g_prep = prepare_jacobian(gfunc!, g, jac_backend, Z̃_∇g, ∇g_context...) mpc.con.i_g .= i_g_old + ∇g = init_diffmat(JNT, jac_backend, ∇g_prep, nZ̃, ng) ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) ∇gfuncs_i! = if nZ̃ == 1 @@ -716,13 +715,13 @@ function get_optim_functions( return update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) end Z̃_∇geq = fill(myNaN, nZ̃) - ∇geq = zeros(JNT, neq, nZ̃) # Jacobian of equality constraints geq ∇geq_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(X̂0), Cache(gc), Cache(g) ) ∇geq_prep = prepare_jacobian(geqfunc!, geq, jac_backend, Z̃_∇geq, ∇geq_context...) + ∇geq = init_diffmat(JNT, jac_backend, ∇geq_prep, nZ̃, neq) ∇geqfuncs! = Vector{Function}(undef, neq) for i in eachindex(∇geqfuncs!) # only multivariate syntax, univariate is impossible since nonlinear equality diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index f01467e04..ef9af7e52 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1362,7 +1362,6 @@ function get_optim_functions( return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) end Z̃_∇J = fill(myNaN, nZ̃) - ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J ∇J_context = ( Cache(V̂), Cache(X̂0), Cache(û0), Cache(ŷ0), @@ -1370,6 +1369,7 @@ function get_optim_functions( Cache(x̄), ) ∇J_prep = prepare_gradient(Jfunc!, grad_backend, Z̃_∇J, ∇J_context...) + ∇J = Vector{JNT}(undef, nZ̃) ∇Jfunc! = if nZ̃ == 1 function (Z̃arg) Z̃_∇J .= Z̃arg @@ -1399,7 +1399,6 @@ function get_optim_functions( return update_simulations!(Z̃, V̂, X̂0, û0, ŷ0, g) end Z̃_∇g = fill(myNaN, nZ̃) - ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of inequality constraints g ∇g_context = ( Cache(V̂), Cache(X̂0), Cache(û0), Cache(ŷ0), @@ -1409,6 +1408,7 @@ function get_optim_functions( estim.con.i_g .= true ∇g_prep = prepare_jacobian(gfunc!, g, jac_backend, Z̃_∇g, ∇g_context...) estim.con.i_g .= i_g_old + ∇g = init_diffmat(JNT, jac_backend, ∇g_prep, nZ̃, ng) ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) ∇gfuncs![i] = if nZ̃ == 1 diff --git a/src/general.jl b/src/general.jl index b9dff27c1..76330c082 100644 --- a/src/general.jl +++ b/src/general.jl @@ -27,6 +27,10 @@ function get_jacobian!(A, buffer::JacobianBuffer, y, x) return ForwardDiff.jacobian!(A, buffer.f!, y, x, buffer.config) 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) + "Termination status that means 'no solution available'." const ERROR_STATUSES = ( JuMP.INFEASIBLE, JuMP.DUAL_INFEASIBLE, JuMP.LOCALLY_INFEASIBLE, From 6f72934be1f098f84ba320dfe3b7e17f0e98c6d7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 17 Mar 2025 17:08:15 -0400 Subject: [PATCH 12/30] debug: correct sparsity detection for `MovingHorizonEstimator` --- src/estimator/mhe/construct.jl | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index ef9af7e52..2d90d43f6 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1406,8 +1406,10 @@ function get_optim_functions( # temporarily enable all the inequality constraints for sparsity pattern detection: i_g_old = copy(estim.con.i_g) estim.con.i_g .= true + estim.Nk .= estim.He ∇g_prep = prepare_jacobian(gfunc!, g, jac_backend, Z̃_∇g, ∇g_context...) estim.con.i_g .= i_g_old + estim.Nk .= 0 ∇g = init_diffmat(JNT, jac_backend, ∇g_prep, nZ̃, ng) ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) From 0e2b8558c447063a2eab745b2000837055a3d23f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 09:19:30 -0400 Subject: [PATCH 13/30] added: `mpc` and `estim` object as `DI.Constant` --- src/controller/nonlinmpc.jl | 61 +++++++++++++++++----------------- src/estimator/mhe/construct.jl | 44 ++++++++++++------------ 2 files changed, 54 insertions(+), 51 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 6e6f8141b..f6f6f336f 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -589,11 +589,9 @@ function get_optim_functions( grad_backend::AbstractADType, jac_backend ::AbstractADType ) where JNT<:Real - model, transcription = mpc.estim.model, mpc.transcription - #TODO: fix type of all cache to ::Vector{JNT} (verify performance difference with and w/o) - #TODO: mêmes choses pour le MHE - # --------------------- update simulation function ------------------------------------ - function update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + # ------ update simulation function (all args after `mpc` are mutated) ---------------- + function update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + model, transcription = mpc.estim.model, mpc.transcription U0 = getU0!(U0, mpc, Z̃) ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃) @@ -605,36 +603,38 @@ function get_optim_functions( return nothing end # ----- common cache for Jfunc, gfuncs, geqfuncs called with floats ------------------- + model = mpc.estim.model nu, ny, nx̂, nϵ, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, mpc.Hp, mpc.Hc ng, nc, neq = length(mpc.con.i_g), mpc.con.nc, mpc.con.neq nZ̃, nU, nŶ, nX̂ = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂ nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny - myNaN = convert(JNT, NaN) - Z̃ = fill(myNaN, nZ̃) # NaN to force update_simulations! at first call - ΔŨ = zeros(JNT, nΔŨ) - x̂0end = zeros(JNT, nx̂) - Ue, Ŷe = zeros(JNT, nUe), zeros(JNT, nŶe) - U0, Ŷ0 = zeros(JNT, nU), zeros(JNT, nŶ) - Û0, X̂0 = zeros(JNT, nU), zeros(JNT, nX̂) - gc, g = zeros(JNT, nc), zeros(JNT, ng) - geq = zeros(JNT, neq) - # ---------------------- objective function ------------------------------------------ + myNaN = convert(JNT, NaN) # NaN to force update_simulations! at first call: + Z̃ ::Vector{JNT} = fill(myNaN, nZ̃) + ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) + x̂0end::Vector{JNT} = zeros(JNT, nx̂) + 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) + # ---------------------- objective function ------------------------------------------- function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) end return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T end - function Jfunc!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) - update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + function Jfunc!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) end Z̃_∇J = fill(myNaN, nZ̃) ∇J_context = ( + Constant(mpc), Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(X̂0), - Cache(gc), Cache(g), Cache(geq) + Cache(gc), Cache(g), Cache(geq), ) ∇J_prep = prepare_gradient(Jfunc!, grad_backend, Z̃_∇J, ∇J_context...) ∇J = Vector{JNT}(undef, nZ̃) @@ -657,26 +657,26 @@ function get_optim_functions( gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) end return g[i]::T end gfuncs[i] = gfunc_i end - function gfunc!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, geq) - return update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + function gfunc!(g, Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, geq) + return update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) end Z̃_∇g = fill(myNaN, nZ̃) ∇g_context = ( + Constant(mpc), Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(X̂0), - Cache(gc), Cache(geq) + Cache(gc), Cache(geq), ) - # temporarily enable all the inequality constraints for sparsity pattern detection: - i_g_old = copy(mpc.con.i_g) - mpc.con.i_g .= true + # temporarily enable all the inequality constraints for sparsity detection: + mpc.con.i_g[1:end-nc] .= true ∇g_prep = prepare_jacobian(gfunc!, g, jac_backend, Z̃_∇g, ∇g_context...) - mpc.con.i_g .= i_g_old + mpc.con.i_g[1:end-nc] .= false ∇g = init_diffmat(JNT, jac_backend, ∇g_prep, nZ̃, ng) ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) @@ -705,17 +705,18 @@ function get_optim_functions( geqfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) end return geq[i]::T end geqfuncs[i] = geqfunc_i end - function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g) - return update_simulations!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + function geqfunc!(geq, Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g) + return update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) end Z̃_∇geq = fill(myNaN, nZ̃) ∇geq_context = ( + Constant(mpc), Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(X̂0), Cache(gc), Cache(g) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 2d90d43f6..80d909e0a 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1332,37 +1332,39 @@ function get_optim_functions( grad_backend::AbstractADType, jac_backend::AbstractADType ) where {JNT <: Real} - model, con = estim.model, estim.con - # --------------------- update simulation function ------------------------------------ - function update_simulations!(Z̃, V̂, X̂0, û0, ŷ0, g) + # -------- update simulation function (all args after `estim` are mutated) ------------ + function update_simulations!(Z̃, estim, V̂, X̂0, û0, ŷ0, g) + model = estim.model V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, Z̃) ϵ = getϵ(estim, Z̃) g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ) return nothing end # ---------- common cache for Jfunc, gfuncs called with floats ------------------------ + model, con = estim.model, estim.con nx̂, nym, nŷ, nu, nϵ, He = estim.nx̂, estim.nym, model.ny, model.nu, estim.nϵ, estim.He nV̂, nX̂, ng, nZ̃ = He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) - myNaN = convert(JNT, NaN) - Z̃ = fill(myNaN, nZ̃) # NaN to force update_simulations! at first call - V̂, X̂0 = zeros(JNT, nV̂), zeros(JNT, nX̂) - û0, ŷ0 = zeros(JNT, nu), zeros(JNT, nŷ) - g = zeros(JNT, ng) - x̄ = zeros(JNT, nx̂) + myNaN = convert(JNT, NaN) # NaN to force update_simulations! at first call + Z̃::Vector{JNT} = fill(myNaN, nZ̃) + V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) + û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) + g::Vector{JNT} = zeros(JNT, ng) + x̄::Vector{JNT} = zeros(JNT, nx̂) # --------------------- objective functions ------------------------------------------- function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_simulations!(Z̃, V̂, X̂0, û0, ŷ0, g) + update_simulations!(Z̃, estim, V̂, X̂0, û0, ŷ0, g) end return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T end - function Jfunc!(Z̃, V̂, X̂0, û0, ŷ0, g, x̄) - update_simulations!(Z̃, V̂, X̂0, û0, ŷ0, g) + function Jfunc!(Z̃, estim, V̂, X̂0, û0, ŷ0, g, x̄) + update_simulations!(Z̃, estim, V̂, X̂0, û0, ŷ0, g) return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) end Z̃_∇J = fill(myNaN, nZ̃) ∇J_context = ( + Constant(estim), Cache(V̂), Cache(X̂0), Cache(û0), Cache(ŷ0), Cache(g), @@ -1389,27 +1391,27 @@ function get_optim_functions( gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_simulations!(Z̃, V̂, X̂0, û0, ŷ0, g) + update_simulations!(Z̃, estim, V̂, X̂0, û0, ŷ0, g) end return g[i]::T end gfuncs[i] = gfunc_i end - function gfunc!(g, Z̃, V̂, X̂0, û0, ŷ0) - return update_simulations!(Z̃, V̂, X̂0, û0, ŷ0, g) + function gfunc!(g, Z̃, estim, V̂, X̂0, û0, ŷ0) + return update_simulations!(Z̃, estim, V̂, X̂0, û0, ŷ0, g) end Z̃_∇g = fill(myNaN, nZ̃) ∇g_context = ( + Constant(estim), Cache(V̂), Cache(X̂0), Cache(û0), Cache(ŷ0), ) - # temporarily enable all the inequality constraints for sparsity pattern detection: - i_g_old = copy(estim.con.i_g) - estim.con.i_g .= true - estim.Nk .= estim.He + # temporarily enable all the inequality constraints for sparsity detection: + estim.con.i_g .= true + estim.Nk[] = He ∇g_prep = prepare_jacobian(gfunc!, g, jac_backend, Z̃_∇g, ∇g_context...) - estim.con.i_g .= i_g_old - estim.Nk .= 0 + estim.con.i_g .= false + estim.Nk[] = 0 ∇g = init_diffmat(JNT, jac_backend, ∇g_prep, nZ̃, ng) ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) From cec29d2d93e8663c7cff72161fb09558d6017596 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 11:31:28 -0400 Subject: [PATCH 14/30] added: using `strict=Val(true)` un DI.jl preparation --- Project.toml | 2 +- src/controller/nonlinmpc.jl | 7 ++++--- src/estimator/mhe/construct.jl | 5 +++-- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/Project.toml b/Project.toml index 81b2b98a8..191d3620b 100644 --- a/Project.toml +++ b/Project.toml @@ -22,7 +22,7 @@ SparseMatrixColorings = "0a514795-09f3-496d-8182-132a7b665d35" [compat] ControlSystemsBase = "1.9" -DifferentiationInterface = "0.6.44" +DifferentiationInterface = "0.6.45" ForwardDiff = "0.10" Ipopt = "1" JuMP = "1.21" diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index f6f6f336f..606562a2d 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -608,6 +608,7 @@ function get_optim_functions( ng, nc, neq = length(mpc.con.i_g), mpc.con.nc, mpc.con.neq nZ̃, nU, nŶ, nX̂ = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂ nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny + strict = Val(true) myNaN = convert(JNT, NaN) # NaN to force update_simulations! at first call: Z̃ ::Vector{JNT} = fill(myNaN, nZ̃) ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) @@ -636,7 +637,7 @@ function get_optim_functions( Cache(Û0), Cache(X̂0), Cache(gc), Cache(g), Cache(geq), ) - ∇J_prep = prepare_gradient(Jfunc!, grad_backend, Z̃_∇J, ∇J_context...) + ∇J_prep = prepare_gradient(Jfunc!, grad_backend, Z̃_∇J, ∇J_context...; strict) ∇J = Vector{JNT}(undef, nZ̃) ∇Jfunc! = if nZ̃ == 1 function (Z̃arg) @@ -675,7 +676,7 @@ function get_optim_functions( ) # temporarily enable all the inequality constraints for sparsity detection: mpc.con.i_g[1:end-nc] .= true - ∇g_prep = prepare_jacobian(gfunc!, g, jac_backend, Z̃_∇g, ∇g_context...) + ∇g_prep = prepare_jacobian(gfunc!, g, jac_backend, Z̃_∇g, ∇g_context...; strict) mpc.con.i_g[1:end-nc] .= false ∇g = init_diffmat(JNT, jac_backend, ∇g_prep, nZ̃, ng) ∇gfuncs! = Vector{Function}(undef, ng) @@ -721,7 +722,7 @@ function get_optim_functions( Cache(Û0), Cache(X̂0), Cache(gc), Cache(g) ) - ∇geq_prep = prepare_jacobian(geqfunc!, geq, jac_backend, Z̃_∇geq, ∇geq_context...) + ∇geq_prep = prepare_jacobian(geqfunc!, geq, jac_backend, Z̃_∇geq, ∇geq_context...; strict) ∇geq = init_diffmat(JNT, jac_backend, ∇geq_prep, nZ̃, neq) ∇geqfuncs! = Vector{Function}(undef, neq) for i in eachindex(∇geqfuncs!) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 80d909e0a..9b68bdfe0 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1345,6 +1345,7 @@ function get_optim_functions( nx̂, nym, nŷ, nu, nϵ, He = estim.nx̂, estim.nym, model.ny, model.nu, estim.nϵ, estim.He nV̂, nX̂, ng, nZ̃ = He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) myNaN = convert(JNT, NaN) # NaN to force update_simulations! at first call + strict = Val(true) Z̃::Vector{JNT} = fill(myNaN, nZ̃) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) @@ -1370,7 +1371,7 @@ function get_optim_functions( Cache(g), Cache(x̄), ) - ∇J_prep = prepare_gradient(Jfunc!, grad_backend, Z̃_∇J, ∇J_context...) + ∇J_prep = prepare_gradient(Jfunc!, grad_backend, Z̃_∇J, ∇J_context...; strict) ∇J = Vector{JNT}(undef, nZ̃) ∇Jfunc! = if nZ̃ == 1 function (Z̃arg) @@ -1409,7 +1410,7 @@ function get_optim_functions( # temporarily enable all the inequality constraints for sparsity detection: estim.con.i_g .= true estim.Nk[] = He - ∇g_prep = prepare_jacobian(gfunc!, g, jac_backend, Z̃_∇g, ∇g_context...) + ∇g_prep = prepare_jacobian(gfunc!, g, jac_backend, Z̃_∇g, ∇g_context...; strict) estim.con.i_g .= false estim.Nk[] = 0 ∇g = init_diffmat(JNT, jac_backend, ∇g_prep, nZ̃, ng) From 711e28621c2c6b23aaea34c41640084203df1219 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 11:54:32 -0400 Subject: [PATCH 15/30] doc: replace urls with inter-links --- docs/src/manual/mtk.md | 4 ++-- src/estimator/kalman.jl | 2 +- src/estimator/mhe/construct.jl | 4 ++-- src/estimator/mhe/execute.jl | 2 +- src/sim_model.jl | 8 ++++---- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/docs/src/manual/mtk.md b/docs/src/manual/mtk.md index 29f616bc8..605ed0af1 100644 --- a/docs/src/manual/mtk.md +++ b/docs/src/manual/mtk.md @@ -11,8 +11,8 @@ old_logger = global_logger(); global_logger(errlogger); ## Pendulum Model -This example integrates the simple pendulum model of the [last section](@ref man_nonlin) in the -[`ModelingToolkit.jl`](https://docs.sciml.ai/ModelingToolkit/stable/) (MTK) framework and +This example integrates the simple pendulum model of the [last section](@ref man_nonlin) in +[`ModelingToolkit`](https://docs.sciml.ai/ModelingToolkit/stable/) (MTK) framework and extracts appropriate `f!` and `h!` functions to construct a [`NonLinModel`](@ref). An [`NonLinMPC`](@ref) is designed from this model and simulated to reproduce the results of the last section. diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 44496422b..43e69e5f3 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -802,7 +802,7 @@ See [`init_ukf`](@ref) for the definition of the constants ``\mathbf{m̂, Ŝ}`` superscript in e.g. ``\mathbf{X̂}_{k-1}^j(k)`` refers the vector at the ``j``th column of ``\mathbf{X̂}_{k-1}(k)``. The symbol ``\mathbf{0}`` is a vector with zeros. The number of sigma points is ``n_σ = 2 n_\mathbf{x̂} + 1``. The matrices ``\sqrt{\mathbf{P̂}_{k-1}(k)}`` -and ``\sqrt{\mathbf{P̂}_{k}(k)}`` are the the lower triangular factors of [`cholesky`](https://docs.julialang.org/en/v1/stdlib/LinearAlgebra/#LinearAlgebra.cholesky) +and ``\sqrt{\mathbf{P̂}_{k}(k)}`` are the the lower triangular factors of [`cholesky`](@extref Julia LinearAlgebra.cholesky) results. The correction and prediction step equations are provided below. The correction step is skipped if `estim.direct == true` since it's already done by the user. diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 9b68bdfe0..4e7f838f9 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1318,13 +1318,13 @@ This method is really intricate and I'm not proud of it. That's because of 3 ele - These functions are used inside the nonlinear optimization, so they must be type-stable and as efficient as possible. - The `JuMP` NLP syntax forces splatting for the decision variable, which implies use - of `Vararg{T,N}` (see the [performance tip](https://docs.julialang.org/en/v1/manual/performance-tips/#Be-aware-of-when-Julia-avoids-specializing)) + of `Vararg{T,N}` (see the [performance tip](@extref Julia Be-aware-of-when-Julia-avoids-specializing)) and memoization to avoid redundant computations. This is already complex, but it's even worse knowing that most automatic differentiation tools do not support splatting. - The signature of gradient and hessian functions is not the same for univariate (`nZ̃ == 1`) and multivariate (`nZ̃ > 1`) operators in `JuMP`. Both must be defined. -Inspired from: [User-defined operators with vector outputs](https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs) +Inspired from: [User-defined operators with vector outputs](@extref JuMP User-defined-operators-with-vector-outputs) """ function get_optim_functions( estim::MovingHorizonEstimator, diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 1afbfdc76..25bf3b5fc 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -379,7 +379,7 @@ where ``\mathbf{ŵ}_{k-1}(k-j)`` is the input increment for time ``k-j`` comput last time step ``k-1``. It then calls `JuMP.optimize!(estim.optim)` and extract the solution. A failed optimization prints an `@error` log in the REPL and returns the warm-start value. A failed optimization also prints [`getinfo`](@ref) results in -the debug log [if activated](https://docs.julialang.org/en/v1/stdlib/Logging/#Example:-Enable-debug-level-messages). +the debug log [if activated](@extref Julia Example:-Enable-debug-level-messages). """ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real model, optim = estim.model, estim.optim diff --git a/src/sim_model.jl b/src/sim_model.jl index aaad33f8f..d169745cd 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -301,10 +301,10 @@ end Sleep for `model.Ts` s minus the time elapsed since the last call to [`savetime!`](@ref). -It calls [`sleep`](https://docs.julialang.org/en/v1/base/parallel/#Base.sleep) if `busywait` -is `false`. Else, a simple `while` loop implements busy-waiting. As a rule-of-thumb, -busy-waiting should be used if `model.Ts < 0.1` s, since the accuracy of `sleep` is around 1 -ms. Can be used to implement simple soft real-time simulations, see the example below. +It calls [`sleep`](@extref Julia Base.sleep) if `busywait` is `false`. Else, a simple +`while` loop implements busy-waiting. As a rule-of-thumb, busy-waiting should be used if +`model.Ts < 0.1` s, since the accuracy of `sleep` is around 1 ms. Can be used to implement +simple soft real-time simulations, see the example below. !!! warning The allocations in Julia are garbage-collected (GC) automatically. This can affect the From 30b2e4d911dd096fee7748f80714b5b4209d6910 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 12:04:47 -0400 Subject: [PATCH 16/30] debug: `NonLinMPC` with `LinModel` and custom constraints now works --- src/controller/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 65737bd72..9e3f5c0e7 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -624,7 +624,7 @@ function init_nonlincon!( for i in 1:con.nc name = Symbol("g_c_$i") optim[name] = JuMP.add_nonlinear_operator( - optim, nZ̃, gfuncs[i_base+i]; name + optim, nZ̃, gfuncs[i_base+i], ∇gfuncs![i_base+i]; name ) end end From b28b7024cfae8baca58eed35db5872c2b2647249 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 12:41:21 -0400 Subject: [PATCH 17/30] changed: `update_prediction!` methods are now not nested It is no longer necessary with `DI.Constant` context. --- src/controller/execute.jl | 9 ++++-- src/controller/nonlinmpc.jl | 50 +++++++++++++++++++++------------- src/estimator/mhe/construct.jl | 16 +++-------- src/estimator/mhe/execute.jl | 16 +++++++++++ 4 files changed, 57 insertions(+), 34 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 408559688..d7cb78b81 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -123,16 +123,16 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real x̂0end = similar(mpc.estim.x̂0) Ue, Ŷe = Vector{NT}(undef, nUe), Vector{NT}(undef, nŶe) U0, Ŷ0 = similar(mpc.Uop), similar(mpc.Yop) - X̂0, Û0 = Vector{NT}(undef, nX̂0), Vector{NT}(undef, nÛ0) + Û0, X̂0 = Vector{NT}(undef, nÛ0), Vector{NT}(undef, nX̂0) U, Ŷ = similar(mpc.Uop), similar(mpc.Yop) - Ŷs = similar(mpc.Yop) U0 = getU0!(U0, mpc, Z̃) - ΔŨ = getΔŨ!(ΔŨ, mpc, mpc.transcription, Z̃) + ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃) Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) U .= U0 .+ mpc.Uop Ŷ .= Ŷ0 .+ mpc.Yop J = obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) + Ŷs = similar(mpc.Yop) predictstoch!(Ŷs, mpc, mpc.estim) info[:ΔU] = Z̃[1:mpc.Hc*model.nu] info[:ϵ] = getϵ(mpc, Z̃) @@ -370,6 +370,9 @@ function obj_nonlinprog!( return JR̂y + JΔŨ + JR̂u + E_JE end +"No custom nonlinear constraints `gc` by default, return `gc` unchanged." +con_custom!(gc, ::PredictiveController, _ , _, _ ) = gc + "By default, the economic term is zero." function obj_econ(::PredictiveController, ::SimModel, _ , ::AbstractVector{NT}) where NT return zero(NT) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 606562a2d..e8d3b0abc 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -589,19 +589,6 @@ function get_optim_functions( grad_backend::AbstractADType, jac_backend ::AbstractADType ) where JNT<:Real - # ------ update simulation function (all args after `mpc` are mutated) ---------------- - function update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) - model, transcription = mpc.estim.model, mpc.transcription - U0 = getU0!(U0, mpc, Z̃) - ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) - Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃) - Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) - ϵ = getϵ(mpc, Z̃) - gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ) - g = con_nonlinprog!(g, mpc, model, transcription, x̂0end, Ŷ0, gc, ϵ) - geq = con_nonlinprogeq!(geq, X̂0, Û0, mpc, model, transcription, U0, Z̃) - return nothing - end # ----- common cache for Jfunc, gfuncs, geqfuncs called with floats ------------------- model = mpc.estim.model nu, ny, nx̂, nϵ, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, mpc.Hp, mpc.Hc @@ -622,12 +609,12 @@ function get_optim_functions( function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) end return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T end function Jfunc!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) - update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) end Z̃_∇J = fill(myNaN, nZ̃) @@ -658,14 +645,14 @@ function get_optim_functions( gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) end return g[i]::T end gfuncs[i] = gfunc_i end function gfunc!(g, Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, geq) - return update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + return update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) end Z̃_∇g = fill(myNaN, nZ̃) ∇g_context = ( @@ -706,14 +693,14 @@ function get_optim_functions( geqfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) end return geq[i]::T end geqfuncs[i] = geqfunc_i end function geqfunc!(geq, Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g) - return update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + return update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) end Z̃_∇geq = fill(myNaN, nZ̃) ∇geq_context = ( @@ -743,6 +730,31 @@ function get_optim_functions( return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! end +""" + update_predictions!( + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, + mpc::PredictiveController, Z̃ + ) -> nothing + +Update in-place all vectors for the predictions of `mpc` controller at decision vector `Z̃`. + +The method mutates all the arguments before the `mpc` argument. +""" +function update_predictions!( + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ +) + model, transcription = mpc.estim.model, mpc.transcription + U0 = getU0!(U0, mpc, Z̃) + ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) + Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃) + Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) + ϵ = getϵ(mpc, Z̃) + gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ) + g = con_nonlinprog!(g, mpc, model, transcription, x̂0end, Ŷ0, gc, ϵ) + geq = con_nonlinprogeq!(geq, X̂0, Û0, mpc, model, transcription, U0, Z̃) + return nothing +end + @doc raw""" con_custom!(gc, mpc::NonLinMPC, Ue, Ŷe, ϵ) -> gc diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 4e7f838f9..dc4b0e9ae 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1332,14 +1332,6 @@ function get_optim_functions( grad_backend::AbstractADType, jac_backend::AbstractADType ) where {JNT <: Real} - # -------- update simulation function (all args after `estim` are mutated) ------------ - function update_simulations!(Z̃, estim, V̂, X̂0, û0, ŷ0, g) - model = estim.model - V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, Z̃) - ϵ = getϵ(estim, Z̃) - g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ) - return nothing - end # ---------- common cache for Jfunc, gfuncs called with floats ------------------------ model, con = estim.model, estim.con nx̂, nym, nŷ, nu, nϵ, He = estim.nx̂, estim.nym, model.ny, model.nu, estim.nϵ, estim.He @@ -1355,12 +1347,12 @@ function get_optim_functions( function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_simulations!(Z̃, estim, V̂, X̂0, û0, ŷ0, g) + update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃) end return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T end function Jfunc!(Z̃, estim, V̂, X̂0, û0, ŷ0, g, x̄) - update_simulations!(Z̃, estim, V̂, X̂0, û0, ŷ0, g) + update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃) return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) end Z̃_∇J = fill(myNaN, nZ̃) @@ -1392,14 +1384,14 @@ function get_optim_functions( gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_simulations!(Z̃, estim, V̂, X̂0, û0, ŷ0, g) + update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃) end return g[i]::T end gfuncs[i] = gfunc_i end function gfunc!(g, Z̃, estim, V̂, X̂0, û0, ŷ0) - return update_simulations!(Z̃, estim, V̂, X̂0, û0, ŷ0, g) + return update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃) end Z̃_∇g = fill(myNaN, nZ̃) ∇g_context = ( diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 25bf3b5fc..63336d393 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -591,6 +591,22 @@ function predict!(V̂, X̂0, û0, ŷ0, estim::MovingHorizonEstimator, model::S return V̂, X̂0 end + +""" + update_predictions!(V̂, X̂0, û0, ŷ0, g, estim::MovingHorizonEstimator, Z̃) + +Update in-place the vectors for the predictions of `estim` estimator at decision vector `Z̃`. + +The method mutates all the arguments before `estim` argument. +""" +function update_prediction!(V̂, X̂0, û0, ŷ0, g, estim::MovingHorizonEstimator, Z̃) + model = estim.model + V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, Z̃) + ϵ = getϵ(estim, Z̃) + g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ) + return nothing +end + """ con_nonlinprog!(g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, ϵ) From 8922d3bd2a26026d4e6feee3949afe8e9352096f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 13:03:43 -0400 Subject: [PATCH 18/30] test: remove useless test with new DI.jl interface --- test/2_test_state_estim.jl | 7 ------- test/3_test_predictive_control.jl | 6 ------ 2 files changed, 13 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index fa2eb408c..fc6af61fd 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -968,13 +968,6 @@ end x̂ = updatestate!(mhe3, [0], [0]) @test x̂ ≈ [0, 0] atol=1e-3 @test isa(x̂, Vector{Float32}) - - mhe4 = setconstraint!(MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0), v̂max=[50,50]) - g_V̂max_end = mhe4.optim[:g_V̂max_2].func - # test gfunc_i(i,::NTuple{N, Float64}) - @test g_V̂max_end(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) ≤ 0.0 - # test gfunc_i(i,::NTuple{N, ForwardDiff.Dual}) : - @test ForwardDiff.gradient(vec->g_V̂max_end(vec...), zeros(8)) ≈ zeros(8) Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2) R̂ = diagm([1, 1].^2) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index d099cb6ba..6c5b89a92 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -683,12 +683,6 @@ end preparestate!(nmpc4, [0], [0]) u = moveinput!(nmpc4, [0], d, R̂u=fill(12, nmpc4.Hp)) @test u ≈ [12] atol=5e-2 - nmpc5 = setconstraint!(NonLinMPC(nonlinmodel, Hp=15, Cwt=Inf), ymin=[1]) - g_Y0min_end = nmpc5.optim[:g_Y0min_15].func - # test gfunc_i(i,::NTuple{N, Float64}): - @test g_Y0min_end(20.0, 10.0) ≤ 0.0 - # test gfunc_i(i,::NTuple{N, ForwardDiff.Dual}) : - @test ForwardDiff.gradient(vec->g_Y0min_end(vec...), [20.0, 10.0]) ≈ [-5, -5] atol=1e-3 linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), 0, 0, 3000.0) nmpc6 = NonLinMPC(linmodel3, Hp=10) preparestate!(nmpc6, [0]) From 825b4490fb8e81ecebf069fac116227a34eb49e5 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 14:43:29 -0400 Subject: [PATCH 19/30] test: re-add the not so useless tests for coverage --- test/2_test_state_estim.jl | 14 ++++++++++---- test/3_test_predictive_control.jl | 17 +++++++++++++---- 2 files changed, 23 insertions(+), 8 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index fc6af61fd..20f6e752b 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -864,10 +864,12 @@ end @testitem "MovingHorizonEstimator estimation and getinfo" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, Ipopt, ForwardDiff - linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2], i_d=[3]), uop=[10,50], yop=[50,30], dop=[5]) - f(x,u,d,_) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d - h(x,d,_) = linmodel1.C*x + linmodel1.Dd*d - nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing), uop=[10,50], yop=[50,30], dop=[5]) + linmodel1 = LinModel(sys,Ts,i_u=[1,2], i_d=[3]) + linmodel1 = setop!(linmodel1, uop=[10,50], yop=[50,30], dop=[5]) + f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d + h(x,d,model) = model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel1) + nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5]) mhe1 = MovingHorizonEstimator(nonlinmodel, He=2) preparestate!(mhe1, [50, 30], [5]) @@ -968,6 +970,10 @@ end x̂ = updatestate!(mhe3, [0], [0]) @test x̂ ≈ [0, 0] atol=1e-3 @test isa(x̂, Vector{Float32}) + mhe4 = setconstraint!(MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0), v̂max=[50,50]) + g_V̂max_end = mhe4.optim[:g_V̂max_2].func + # execute update_predictions! branch in `gfunc_i` for coverage: + @test_nowarn g_V̂max_end(0.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) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 6c5b89a92..c35a222ee 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -661,9 +661,9 @@ end u = moveinput!(nmpc) @test u ≈ [4] atol=5e-2 linmodel2 = LinModel([tf(5, [2000, 1]) tf(7, [8000,1])], 3000.0, i_d=[2]) - f = (x,u,d,_) -> linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d - h = (x,d,_) -> linmodel2.C*x + linmodel2.Dd*d - nonlinmodel = NonLinModel(f, h, 3000.0, 1, 2, 1, 1, solver=nothing) + f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d + h = (x,d,model) -> model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, 3000.0, 1, 2, 1, 1, solver=nothing, p=linmodel2) nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=100, Hc=1) preparestate!(nmpc2, [0], [0]) # if d=[0.1], the output will eventually reach 7*0.1=0.7, no action needed (u=0): @@ -684,13 +684,22 @@ end u = moveinput!(nmpc4, [0], d, R̂u=fill(12, nmpc4.Hp)) @test u ≈ [12] atol=5e-2 linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), 0, 0, 3000.0) + nmpc5 = NonLinMPC(nonlinmodel, Hp=1, Hc=1, Cwt=Inf, transcription=MultipleShooting()) + nmpc5 = setconstraint!(nmpc5, ymin=[1]) + # execute update_predictions! branch in `gfunc_i` for coverage: + g_Y0min_end = nmpc5.optim[:g_Y0min_1].func + println(nmpc5.Z̃) + @test_nowarn g_Y0min_end(10.0, 9.0, 8.0, 7.0) + # execute update_predictions! branch in `geqfunc_i` for coverage: + geq_end = nmpc5.optim[:geq_2].func + @test_nowarn geq_end(5.0, 4.0, 3.0, 2.0) nmpc6 = NonLinMPC(linmodel3, Hp=10) preparestate!(nmpc6, [0]) @test moveinput!(nmpc6, [0]) ≈ [0.0] nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing) nmpc7 = NonLinMPC(nonlinmodel2, Hp=10) y = similar(nonlinmodel2.yop) - nonlinmodel2.h!(y, Float32[0,0], Float32[0], Float32[]) + nonlinmodel2.h!(y, Float32[0,0], Float32[0], nonlinmodel2.p) preparestate!(nmpc7, [0], [0]) @test moveinput!(nmpc7, [0], [0]) ≈ [0.0] nmpc8 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=100, Hc=1, transcription=MultipleShooting()) From 4a7c4db4ef50dcb263b0a8f1c45a152d4f64323a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 14:52:05 -0400 Subject: [PATCH 20/30] changed: remove unreachable branch in `MovingHorizonEstimator` --- src/estimator/mhe/construct.jl | 40 +++++++++++----------------------- 1 file changed, 13 insertions(+), 27 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index dc4b0e9ae..f98bf0fad 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1365,19 +1365,14 @@ function get_optim_functions( ) ∇J_prep = prepare_gradient(Jfunc!, grad_backend, Z̃_∇J, ∇J_context...; strict) ∇J = Vector{JNT}(undef, nZ̃) - ∇Jfunc! = if nZ̃ == 1 - function (Z̃arg) - Z̃_∇J .= Z̃arg - gradient!(Jfunc!, ∇J, ∇J_prep, grad_backend, Z̃_∇J, ∇J_context...) - return ∇J[begin] # univariate syntax, see JuMP.@operator doc - end - else - function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} - Z̃_∇J .= Z̃arg - gradient!(Jfunc!, ∇J, ∇J_prep, grad_backend, Z̃_∇J, ∇J_context...) - return ∇J # multivariate syntax, see JuMP.@operator doc - end + ∇Jfunc! = function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} + # only the multivariate syntax of JuMP.@operator, univariate is impossible for MHE + # since Z̃ comprises the arrival state estimate AND the estimated process noise + Z̃_∇J .= Z̃arg + gradient!(Jfunc!, ∇J, ∇J_prep, grad_backend, Z̃_∇J, ∇J_context...) + return ∇J end + # --------------------- inequality constraint functions ------------------------------- gfuncs = Vector{Function}(undef, ng) for i in eachindex(gfuncs) @@ -1408,22 +1403,13 @@ function get_optim_functions( ∇g = init_diffmat(JNT, jac_backend, ∇g_prep, nZ̃, ng) ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) - ∇gfuncs![i] = if nZ̃ == 1 - function (Z̃arg::T) where T<:Real - if isdifferent(Z̃arg, Z̃_∇g) - Z̃_∇g .= Z̃arg - jacobian!(gfunc!, g, ∇g, ∇g_prep, jac_backend, Z̃_∇g. ∇g_context...) - end - return ∇g[i, begin] # univariate syntax, see JuMP.@operator doc - end - else - function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} - if isdifferent(Z̃arg, Z̃_∇g) - Z̃_∇g .= Z̃arg - jacobian!(gfunc!, g, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context...) - end - return ∇g_i .= @views ∇g[i, :] # multivariate syntax, see JuMP.@operator doc + ∇gfuncs![i] = function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} + # only the multivariate syntax of JuMP.@operator, see above for the explanation + if isdifferent(Z̃arg, Z̃_∇g) + Z̃_∇g .= Z̃arg + jacobian!(gfunc!, g, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context...) end + return ∇g_i .= @views ∇g[i, :] end end return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! From d0e42c29dcdaa54b3b2be7bdf5472ad5e3876c76 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 14:54:04 -0400 Subject: [PATCH 21/30] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 191d3620b..a30510683 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "1.4.4" +version = "1.5.0" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" From 5684d43eb43e614d8364cf73dcb530f3abe6c101 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 15:03:30 -0400 Subject: [PATCH 22/30] doc: DI.jl mention in doc homepage --- docs/src/index.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/docs/src/index.md b/docs/src/index.md index 278fb1efd..ed5b00213 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -12,8 +12,9 @@ The objective is to provide a simple, clear and modular framework to quickly des predictive controllers (MPCs) in Julia, while preserving the flexibility for advanced real-time optimization. Modern MPCs based on closed-loop state estimators are the main focus of the package, but classical approaches that rely on internal models are also possible. The -`JuMP.jl` interface allows the user to test different solvers easily if the performance of -the default settings is not satisfactory. +`JuMP` amd `DifferentiationInterface` dependencies allows the user to test different +optimizers and automatic differentiation (AD) backends easily if the performance of the +default settings is not satisfactory. The documentation is divided in two parts: From 52526e1622e9e17e65299bc1ba7c5745f8e92731 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 15:04:02 -0400 Subject: [PATCH 23/30] doc: idem --- docs/src/index.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/src/index.md b/docs/src/index.md index ed5b00213..4668591ac 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -13,8 +13,8 @@ predictive controllers (MPCs) in Julia, while preserving the flexibility for adv real-time optimization. Modern MPCs based on closed-loop state estimators are the main focus of the package, but classical approaches that rely on internal models are also possible. The `JuMP` amd `DifferentiationInterface` dependencies allows the user to test different -optimizers and automatic differentiation (AD) backends easily if the performance of the -default settings is not satisfactory. +optimizers and automatic differentiation (AD) backends easily if the performances of the +default settings are not satisfactory. The documentation is divided in two parts: From 6095a156b0fd5949154d52779336982f0ee60e90 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 15:13:32 -0400 Subject: [PATCH 24/30] test: debug nmpc test --- test/3_test_predictive_control.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index c35a222ee..2d7ff4f8d 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -696,7 +696,7 @@ end nmpc6 = NonLinMPC(linmodel3, Hp=10) preparestate!(nmpc6, [0]) @test moveinput!(nmpc6, [0]) ≈ [0.0] - nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing) + nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing, p=linmodel2) nmpc7 = NonLinMPC(nonlinmodel2, Hp=10) y = similar(nonlinmodel2.yop) nonlinmodel2.h!(y, Float32[0,0], Float32[0], nonlinmodel2.p) From 1a1a4740def025b84ed70582d22c1069c5d41619 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 15:40:42 -0400 Subject: [PATCH 25/30] test: new test NMPC and MHE tests with `FiniteDiff` --- Project.toml | 3 ++- docs/src/index.md | 2 +- test/3_test_predictive_control.jl | 12 ++++++++++-- 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/Project.toml b/Project.toml index a30510683..8be585cce 100644 --- a/Project.toml +++ b/Project.toml @@ -41,10 +41,11 @@ julia = "1.10" [extras] DAQP = "c47d62df-3981-49c8-9651-128b1cd08617" Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4" +FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41" Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" TestItemRunner = "f8b46487-2199-4994-9208-9a1283c18c0a" TestItems = "1c621080-faea-4a02-84b6-bbd5e436b8fe" [targets] -test = ["Test", "TestItems", "TestItemRunner", "Documenter", "Plots", "DAQP"] +test = ["Test", "TestItems", "TestItemRunner", "Documenter", "Plots", "DAQP", "FiniteDiff"] diff --git a/docs/src/index.md b/docs/src/index.md index 4668591ac..4be5bb160 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -12,7 +12,7 @@ The objective is to provide a simple, clear and modular framework to quickly des predictive controllers (MPCs) in Julia, while preserving the flexibility for advanced real-time optimization. Modern MPCs based on closed-loop state estimators are the main focus of the package, but classical approaches that rely on internal models are also possible. The -`JuMP` amd `DifferentiationInterface` dependencies allows the user to test different +`JuMP` and `DifferentiationInterface` dependencies allows the user to test different optimizers and automatic differentiation (AD) backends easily if the performances of the default settings are not satisfactory. diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 2d7ff4f8d..b3fb3a89c 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -555,7 +555,9 @@ end end @testitem "NonLinMPC construction" setup=[SetupMPCtests] begin - using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, Ipopt + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt, DifferentiationInterface + import FiniteDiff linmodel1 = LinModel(sys,Ts,i_d=[3]) nmpc0 = NonLinMPC(linmodel1, Hp=15) @test isa(nmpc0.estim, SteadyKalmanFilter) @@ -612,6 +614,10 @@ end @test nmpc17.transcription == MultipleShooting() @test length(nmpc17.Z̃) == linmodel1.nu*nmpc17.Hc + nmpc17.estim.nx̂*nmpc17.Hp + nmpc17.nϵ @test size(nmpc17.con.Aeq, 1) == nmpc17.estim.nx̂*nmpc17.Hp + @test_nowarn nmpc18 = NonLinMPC(nonlinmodel, Hp=10, + gradient=AutoFiniteDiff(), + jacobian=AutoFiniteDiff() + ) nonlinmodel2 = NonLinModel{Float32}(f, h, Ts, 2, 4, 2, 1, solver=nothing) nmpc15 = NonLinMPC(nonlinmodel2, Hp=15) @@ -629,7 +635,9 @@ end end @testitem "NonLinMPC moves and getinfo" setup=[SetupMPCtests] begin - using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using DifferentiationInterface + import FiniteDiff linmodel = setop!(LinModel(tf(5, [2000, 1]), 3000.0), yop=[10]) Hp = 100 nmpc_lin = NonLinMPC(linmodel, Nwt=[0], Hp=Hp, Hc=1) From 77f0b140c4438a026758cd9f11584d5b3fd24f71 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 16:10:54 -0400 Subject: [PATCH 26/30] added: store `AbstractADType` backends as struct parameters This is "safer" like that. And it allows the user to verify what are the DI backends in a given MPC/MHE object. Also useful for the tests. --- src/controller/nonlinmpc.jl | 65 ++++++++++++----------------- src/estimator/mhe/construct.jl | 75 +++++++++++++++------------------- 2 files changed, 60 insertions(+), 80 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index e8d3b0abc..1edfd33b3 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -12,7 +12,9 @@ struct NonLinMPC{ NT<:Real, SE<:StateEstimator, TM<:TranscriptionMethod, - JM<:JuMP.GenericModel, + JM<:JuMP.GenericModel, + GB<:AbstractADType, + JB<:AbstractADType, PT<:Any, JEfunc<:Function, GCfunc<:Function @@ -23,6 +25,8 @@ struct NonLinMPC{ # different since solvers that support non-Float64 are scarce. optim::JM con::ControllerConstraint{NT, GCfunc} + gradient::GB + jacobian::JB Z̃::Vector{NT} ŷ::Vector{NT} Hp::Int @@ -59,12 +63,14 @@ struct NonLinMPC{ function NonLinMPC{NT}( estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gc!::GCfunc, nc, p::PT, - transcription::TM, optim::JM, gradient, jacobian + transcription::TM, optim::JM, gradient::GB, jacobian::JB ) where { NT<:Real, SE<:StateEstimator, TM<:TranscriptionMethod, JM<:JuMP.GenericModel, + GB<:AbstractADType, + JB<:AbstractADType, PT<:Any, JEfunc<:Function, GCfunc<:Function, @@ -101,8 +107,9 @@ struct NonLinMPC{ nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ Z̃ = zeros(NT, nZ̃) buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ) - mpc = new{NT, SE, TM, JM, PT, JEfunc, GCfunc}( + mpc = new{NT, SE, TM, JM, GB, JB, PT, JEfunc, GCfunc}( estim, transcription, optim, con, + gradient, jacobian, Z̃, ŷ, Hp, Hc, nϵ, weights, @@ -116,7 +123,7 @@ struct NonLinMPC{ Uop, Yop, Dop, buffer ) - init_optimization!(mpc, model, optim, gradient, jacobian) + init_optimization!(mpc, model, optim) return mpc end end @@ -505,23 +512,11 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real end """ - init_optimization!( - mpc::NonLinMPC, - model::SimModel, - optim::JuMP.GenericModel, - gradient::AbstractADType, - Jacobian::AbstractADType - ) -> nothing + init_optimization!(mpc::NonLinMPC, model::SimModel, optim::JuMP.GenericModel) -> nothing Init the nonlinear optimization for [`NonLinMPC`](@ref) controllers. """ -function init_optimization!( - mpc::NonLinMPC, - model::SimModel, - optim::JuMP.GenericModel, - gradient::AbstractADType, - jacobian::AbstractADType - ) +function init_optimization!(mpc::NonLinMPC, model::SimModel, optim::JuMP.GenericModel) # --- variables and linear constraints --- con, transcription = mpc.con, mpc.transcription nZ̃ = length(mpc.Z̃) @@ -546,7 +541,7 @@ function init_optimization!( end end Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! = get_optim_functions( - mpc, optim, gradient, jacobian + mpc, optim ) @operator(optim, J, nZ̃, Jfunc, ∇Jfunc!) @objective(optim, Min, J(Z̃var...)) @@ -557,10 +552,7 @@ end """ get_optim_functions( - mpc::NonLinMPC, - optim::JuMP.GenericModel, - grad_backend::AbstractADType, - jac_backend ::AbstractADType + mpc::NonLinMPC, optim::JuMP.GenericModel ) -> Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! Return the functions for the nonlinear optimization of `mpc` [`NonLinMPC`](@ref) controller. @@ -583,12 +575,7 @@ This method is really intricate and I'm not proud of it. That's because of 3 ele Inspired from: [User-defined operators with vector outputs](@extref JuMP User-defined-operators-with-vector-outputs) """ -function get_optim_functions( - mpc::NonLinMPC, - optim::JuMP.GenericModel{JNT}, - grad_backend::AbstractADType, - jac_backend ::AbstractADType -) where JNT<:Real +function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real # ----- common cache for Jfunc, gfuncs, geqfuncs called with floats ------------------- model = mpc.estim.model nu, ny, nx̂, nϵ, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, mpc.Hp, mpc.Hc @@ -624,18 +611,18 @@ function get_optim_functions( Cache(Û0), Cache(X̂0), Cache(gc), Cache(g), Cache(geq), ) - ∇J_prep = prepare_gradient(Jfunc!, grad_backend, Z̃_∇J, ∇J_context...; strict) + ∇J_prep = prepare_gradient(Jfunc!, mpc.gradient, Z̃_∇J, ∇J_context...; strict) ∇J = Vector{JNT}(undef, nZ̃) ∇Jfunc! = if nZ̃ == 1 function (Z̃arg) Z̃_∇J .= Z̃arg - gradient!(Jfunc!, ∇J, ∇J_prep, grad_backend, Z̃_∇J, ∇J_context...) + gradient!(Jfunc!, ∇J, ∇J_prep, mpc.gradient, Z̃_∇J, ∇J_context...) return ∇J[begin] # univariate syntax, see JuMP.@operator doc end else function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} Z̃_∇J .= Z̃arg - gradient!(Jfunc!, ∇J, ∇J_prep, grad_backend, Z̃_∇J, ∇J_context...) + gradient!(Jfunc!, ∇J, ∇J_prep, mpc.gradient, Z̃_∇J, ∇J_context...) return ∇J # multivariate syntax, see JuMP.@operator doc end end @@ -663,16 +650,16 @@ function get_optim_functions( ) # temporarily enable all the inequality constraints for sparsity detection: mpc.con.i_g[1:end-nc] .= true - ∇g_prep = prepare_jacobian(gfunc!, g, jac_backend, Z̃_∇g, ∇g_context...; strict) + ∇g_prep = prepare_jacobian(gfunc!, g, mpc.jacobian, Z̃_∇g, ∇g_context...; strict) mpc.con.i_g[1:end-nc] .= false - ∇g = init_diffmat(JNT, jac_backend, ∇g_prep, nZ̃, ng) + ∇g = init_diffmat(JNT, mpc.jacobian, ∇g_prep, nZ̃, ng) ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) ∇gfuncs_i! = if nZ̃ == 1 function (Z̃arg::T) where T<:Real if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!(gfunc!, g, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context...) + jacobian!(gfunc!, g, ∇g, ∇g_prep, mpc.jacobian, Z̃_∇g, ∇g_context...) end return ∇g[i, begin] # univariate syntax, see JuMP.@operator doc end @@ -680,7 +667,7 @@ function get_optim_functions( function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!(gfunc!, g, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context...) + jacobian!(gfunc!, g, ∇g, ∇g_prep, mpc.jacobian, Z̃_∇g, ∇g_context...) end return ∇g_i .= @views ∇g[i, :] # multivariate syntax, see JuMP.@operator doc end @@ -709,8 +696,8 @@ function get_optim_functions( Cache(Û0), Cache(X̂0), Cache(gc), Cache(g) ) - ∇geq_prep = prepare_jacobian(geqfunc!, geq, jac_backend, Z̃_∇geq, ∇geq_context...; strict) - ∇geq = init_diffmat(JNT, jac_backend, ∇geq_prep, nZ̃, neq) + ∇geq_prep = prepare_jacobian(geqfunc!, geq, mpc.jacobian, Z̃_∇geq, ∇geq_context...; strict) + ∇geq = init_diffmat(JNT, mpc.jacobian, ∇geq_prep, nZ̃, neq) ∇geqfuncs! = Vector{Function}(undef, neq) for i in eachindex(∇geqfuncs!) # only multivariate syntax, univariate is impossible since nonlinear equality @@ -720,7 +707,7 @@ function get_optim_functions( if isdifferent(Z̃arg, Z̃_∇geq) Z̃_∇geq .= Z̃arg jacobian!( - geqfunc!, geq, ∇geq, ∇geq_prep, jac_backend, Z̃_∇geq, ∇geq_context... + geqfunc!, geq, ∇geq, ∇geq_prep, mpc.jacobian, Z̃_∇geq, ∇geq_context... ) end return ∇geq_i .= @views ∇geq[i, :] diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index f98bf0fad..c7ec632b5 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -48,6 +48,8 @@ struct MovingHorizonEstimator{ NT<:Real, SM<:SimModel, JM<:JuMP.GenericModel, + GB<:AbstractADType, + JB<:AbstractADType, CE<:StateEstimator, } <: StateEstimator{NT} model::SM @@ -55,6 +57,8 @@ struct MovingHorizonEstimator{ # different since solvers that support non-Float64 are scarce. optim::JM con::EstimatorConstraint{NT} + gradient::GB + jacobian::JB covestim::CE Z̃::Vector{NT} lastu0::Vector{NT} @@ -112,9 +116,16 @@ struct MovingHorizonEstimator{ function MovingHorizonEstimator{NT}( model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt, - optim::JM, gradient, jacobian, covestim::CE; + optim::JM, gradient::GB, jacobian::JB, covestim::CE; direct=true - ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}} + ) where { + NT<:Real, + SM<:SimModel{NT}, + JM<:JuMP.GenericModel, + GB<:AbstractADType, + JB<:AbstractADType, + CE<:StateEstimator{NT} + } nu, ny, nd = model.nu, model.ny, model.nd He < 1 && throw(ArgumentError("Estimation horizon He should be ≥ 1")) Cwt < 0 && throw(ArgumentError("Cwt weight should be ≥ 0")) @@ -158,8 +169,10 @@ struct MovingHorizonEstimator{ P̂arr_old = copy(P̂_0) Nk = [0] corrected = [false] - estim = new{NT, SM, JM, CE}( - model, optim, con, covestim, + estim = new{NT, SM, JM, GB, JB, CE}( + model, optim, con, + gradient, jacobian, + covestim, Z̃, lastu0, x̂op, f̂op, x̂0, He, nϵ, i_ym, nx̂, nym, nyu, nxs, @@ -173,7 +186,7 @@ struct MovingHorizonEstimator{ direct, corrected, buffer ) - init_optimization!(estim, model, optim, gradient, jacobian) + init_optimization!(estim, model, optim) return estim end end @@ -261,9 +274,9 @@ transcription for now. nonlinear optimizer for solving (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl), or [`OSQP`](https://osqp.org/docs/parsers/jump.html) if `model` is a [`LinModel`](@ref)). - `gradient=AutoForwardDiff()` : an `AbstractADType` backend for the gradient of the objective - function if `model` is not a [`LinModel`](@ref), see [`DifferentiationInterface` doc](@extref DifferentiationInterface List). + function when `model` is not a [`LinModel`](@ref), see [`DifferentiationInterface` doc](@extref DifferentiationInterface List). - `jacobian=AutoForwardDiff()` : an `AbstractADType` backend for the Jacobian of the - constraints if `model` is not a [`LinModel`](@ref), see `gradient` above for the options. + constraints when `model` is not a [`LinModel`](@ref), see `gradient` above for the options. - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current estimator, in opposition to the delayed/predictor form). @@ -1197,22 +1210,18 @@ end """ init_optimization!( - estim::MovingHorizonEstimator, model::SimModel, optim::JuMP.GenericModel, _ , _ + estim::MovingHorizonEstimator, model::LinModel, optim::JuMP.GenericModel ) Init the quadratic optimization of [`MovingHorizonEstimator`](@ref). """ function init_optimization!( - estim::MovingHorizonEstimator, - ::LinModel, - optim::JuMP.GenericModel, - ::AbstractADType, - ::AbstractADType + estim::MovingHorizonEstimator, model::LinModel, optim::JuMP.GenericModel, ) nZ̃ = length(estim.Z̃) JuMP.num_variables(optim) == 0 || JuMP.empty!(optim) JuMP.set_silent(optim) - limit_solve_time(estim.optim, estim.model.Ts) + limit_solve_time(optim, model.Ts) @variable(optim, Z̃var[1:nZ̃]) A = estim.con.A[estim.con.i_b, :] b = estim.con.b[estim.con.i_b] @@ -1223,28 +1232,20 @@ end """ init_optimization!( - estim::MovingHorizonEstimator, - model::SimModel, - optim::JuMP.GenericModel, - gradient::AbstractADType, - jacobian::AbstractADType + estim::MovingHorizonEstimator, model::SimModel, optim::JuMP.GenericModel, ) -> nothing Init the nonlinear optimization of [`MovingHorizonEstimator`](@ref). """ function init_optimization!( - estim::MovingHorizonEstimator, - model::SimModel, - optim::JuMP.GenericModel{JNT}, - gradient::AbstractADType, - jacobian::AbstractADType + estim::MovingHorizonEstimator, model::SimModel, optim::JuMP.GenericModel{JNT} ) where JNT<:Real C, con = estim.C, estim.con nZ̃ = length(estim.Z̃) # --- variables and linear constraints --- JuMP.num_variables(optim) == 0 || JuMP.empty!(optim) JuMP.set_silent(optim) - limit_solve_time(estim.optim, estim.model.Ts) + limit_solve_time(optim, model.Ts) @variable(optim, Z̃var[1:nZ̃]) A = estim.con.A[con.i_b, :] b = estim.con.b[con.i_b] @@ -1258,9 +1259,7 @@ function init_optimization!( JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) end end - Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! = get_optim_functions( - estim, optim, gradient, jacobian - ) + Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! = get_optim_functions(estim, optim) @operator(optim, J, nZ̃, Jfunc, ∇Jfunc!) @objective(optim, Min, J(Z̃var...)) nV̂, nX̂ = estim.He*estim.nym, estim.He*estim.nx̂ @@ -1301,10 +1300,7 @@ end """ get_optim_functions( - estim::MovingHorizonEstimator, - optim::JuMP.GenericModel, - grad_backend::AbstractADType, - jac_backend::AbstractADType + estim::MovingHorizonEstimator, optim::JuMP.GenericModel, ) -> Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! Return the functions for the nonlinear optimization of [`MovingHorizonEstimator`](@ref). @@ -1327,10 +1323,7 @@ This method is really intricate and I'm not proud of it. That's because of 3 ele Inspired from: [User-defined operators with vector outputs](@extref JuMP User-defined-operators-with-vector-outputs) """ function get_optim_functions( - estim::MovingHorizonEstimator, - optim::JuMP.GenericModel{JNT}, - grad_backend::AbstractADType, - jac_backend::AbstractADType + estim::MovingHorizonEstimator, ::JuMP.GenericModel{JNT}, ) where {JNT <: Real} # ---------- common cache for Jfunc, gfuncs called with floats ------------------------ model, con = estim.model, estim.con @@ -1363,13 +1356,13 @@ function get_optim_functions( Cache(g), Cache(x̄), ) - ∇J_prep = prepare_gradient(Jfunc!, grad_backend, Z̃_∇J, ∇J_context...; strict) + ∇J_prep = prepare_gradient(Jfunc!, estim.gradient, Z̃_∇J, ∇J_context...; strict) ∇J = Vector{JNT}(undef, nZ̃) ∇Jfunc! = function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} # only the multivariate syntax of JuMP.@operator, univariate is impossible for MHE # since Z̃ comprises the arrival state estimate AND the estimated process noise Z̃_∇J .= Z̃arg - gradient!(Jfunc!, ∇J, ∇J_prep, grad_backend, Z̃_∇J, ∇J_context...) + gradient!(Jfunc!, ∇J, ∇J_prep, estim.gradient, Z̃_∇J, ∇J_context...) return ∇J end @@ -1397,17 +1390,17 @@ function get_optim_functions( # temporarily enable all the inequality constraints for sparsity detection: estim.con.i_g .= true estim.Nk[] = He - ∇g_prep = prepare_jacobian(gfunc!, g, jac_backend, Z̃_∇g, ∇g_context...; strict) + ∇g_prep = prepare_jacobian(gfunc!, g, estim.jacobian, Z̃_∇g, ∇g_context...; strict) estim.con.i_g .= false estim.Nk[] = 0 - ∇g = init_diffmat(JNT, jac_backend, ∇g_prep, nZ̃, ng) + ∇g = init_diffmat(JNT, estim.jacobian, ∇g_prep, nZ̃, ng) ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) ∇gfuncs![i] = function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} # only the multivariate syntax of JuMP.@operator, see above for the explanation if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!(gfunc!, g, ∇g, ∇g_prep, jac_backend, Z̃_∇g, ∇g_context...) + jacobian!(gfunc!, g, ∇g, ∇g_prep, estim.jacobian, Z̃_∇g, ∇g_context...) end return ∇g_i .= @views ∇g[i, :] end From 6cbbbce1fe4fa980101f2c82bf2239476a16f91f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 17:11:47 -0400 Subject: [PATCH 27/30] test: new tests with `AutoFiniteDiff` debug: temporarily fake a "filled" window for MHE gradient preparation (needed for some backend like FiniteDiff) --- src/estimator/mhe/construct.jl | 3 +++ src/estimator/mhe/execute.jl | 11 +++++++++-- test/2_test_state_estim.jl | 16 ++++++++++++---- test/3_test_predictive_control.jl | 17 ++++++++++++++++- 4 files changed, 40 insertions(+), 7 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index c7ec632b5..45ea71b61 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1356,7 +1356,10 @@ function get_optim_functions( Cache(g), Cache(x̄), ) + # temporarily "fill" the estimation window for the preperation of the gradient: + estim.Nk[] = He ∇J_prep = prepare_gradient(Jfunc!, estim.gradient, Z̃_∇J, ∇J_context...; strict) + estim.Nk[] = 0 ∇J = Vector{JNT}(undef, nZ̃) ∇Jfunc! = function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} # only the multivariate syntax of JuMP.@operator, univariate is impossible for MHE diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 63336d393..ba84fc89b 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -522,14 +522,21 @@ Objective function of the MHE when `model` is not a [`LinModel`](@ref). The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. This method mutates `x̄` vector arguments. """ -function obj_nonlinprog!(x̄, estim::MovingHorizonEstimator, ::SimModel, V̂, Z̃) +function obj_nonlinprog!( + x̄, estim::MovingHorizonEstimator, ::SimModel, V̂, Z̃::AbstractVector{NT} +) where NT<:Real nϵ, Nk = estim.nϵ, estim.Nk[] nYm, nŴ, nx̂, invP̄ = Nk*estim.nym, Nk*estim.nx̂, estim.nx̂, estim.invP̄ nx̃ = nϵ + nx̂ invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm] x̂0arr, Ŵ, V̂ = @views Z̃[nx̃-nx̂+1:nx̃], Z̃[nx̃+1:nx̃+nŴ], V̂[1:nYm] x̄ .= estim.x̂0arr_old .- x̂0arr - Jϵ = nϵ ≠ 0 ? estim.C*Z̃[begin]^2 : 0 + Jϵ = nϵ ≠ 0 ? estim.C*Z̃[begin]^2 : zero(NT) + #println(x̂0arr) + #println(invP̄) + #println(dot(x̄, invP̄, x̄)) + #println(dot(Ŵ, invQ̂_Nk, Ŵ)) + #println(dot(V̂, invR̂_Nk, V̂)) return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + Jϵ end diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 20f6e752b..5a807ad81 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -781,11 +781,13 @@ end end @testitem "MovingHorizonEstimator construction" setup=[SetupMPCtests] begin - using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, Ipopt + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt, DifferentiationInterface + import FiniteDiff linmodel1 = LinModel(sys,Ts,i_d=[3]) - f(x,u,d,_) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d - h(x,d,_) = linmodel1.C*x + linmodel1.Du*d - nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing) + f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d + h(x,d,model) = model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel1) mhe1 = MovingHorizonEstimator(linmodel1, He=5) @test mhe1.nym == 2 @@ -857,6 +859,12 @@ end mhe13 = MovingHorizonEstimator(linmodel2, He=5) @test isa(mhe13, MovingHorizonEstimator{Float32}) + mhe14 = MovingHorizonEstimator( + nonlinmodel, He=5, gradient=AutoFiniteDiff(), jacobian=AutoFiniteDiff() + ) + @test mhe14.gradient == AutoFiniteDiff() + @test mhe14.jacobian == AutoFiniteDiff() + @test_throws ArgumentError MovingHorizonEstimator(linmodel1) @test_throws ArgumentError MovingHorizonEstimator(linmodel1, He=0) @test_throws ArgumentError MovingHorizonEstimator(linmodel1, Cwt=-1) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index b3fb3a89c..5dc8e7458 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -614,10 +614,12 @@ end @test nmpc17.transcription == MultipleShooting() @test length(nmpc17.Z̃) == linmodel1.nu*nmpc17.Hc + nmpc17.estim.nx̂*nmpc17.Hp + nmpc17.nϵ @test size(nmpc17.con.Aeq, 1) == nmpc17.estim.nx̂*nmpc17.Hp - @test_nowarn nmpc18 = NonLinMPC(nonlinmodel, Hp=10, + nmpc18 = NonLinMPC(nonlinmodel, Hp=10, gradient=AutoFiniteDiff(), jacobian=AutoFiniteDiff() ) + @test nmpc18.gradient == AutoFiniteDiff() + @test nmpc18.jacobian == AutoFiniteDiff() nonlinmodel2 = NonLinModel{Float32}(f, h, Ts, 2, 4, 2, 1, solver=nothing) nmpc15 = NonLinMPC(nonlinmodel2, Hp=15) @@ -724,6 +726,19 @@ end info = getinfo(nmpc9) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ 20 atol=5e-2 + nmpc10 = setconstraint!(NonLinMPC( + nonlinmodel, Nwt=[0], Hp=100, Hc=1, + gradient=AutoFiniteDiff(), + jacobian=AutoFiniteDiff()), + ymax=[100], ymin=[-100] + ) + preparestate!(nmpc10, [0], [0]) + u = moveinput!(nmpc10, [10], [0]) + @test u ≈ [2] atol=5e-2 + info = getinfo(nmpc10) + @test info[:u] ≈ u + @test info[:Ŷ][end] ≈ 10 atol=5e-2 + @test_nowarn ModelPredictiveControl.info2debugstr(info) end From c4a626dc8787bec6a895b2220435a8a033bad4d9 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 17:19:00 -0400 Subject: [PATCH 28/30] debug: ambiguous methods in MHE --- src/estimator/mhe/execute.jl | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index ba84fc89b..de73b9411 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -510,7 +510,9 @@ Objective function of [`MovingHorizonEstimator`](@ref) when `model` is a [`LinMo It can be called on a [`MovingHorizonEstimator`](@ref) object to evaluate the objective function at specific `Z̃` and `V̂` values. """ -function obj_nonlinprog!( _ , estim::MovingHorizonEstimator, ::LinModel, _ , Z̃) +function obj_nonlinprog!( + _ , estim::MovingHorizonEstimator, ::LinModel, _ , Z̃::AbstractVector{NT} +) where NT<:Real return obj_quadprog(Z̃, estim.H̃, estim.q̃) + estim.r[] end @@ -532,11 +534,6 @@ function obj_nonlinprog!( x̂0arr, Ŵ, V̂ = @views Z̃[nx̃-nx̂+1:nx̃], Z̃[nx̃+1:nx̃+nŴ], V̂[1:nYm] x̄ .= estim.x̂0arr_old .- x̂0arr Jϵ = nϵ ≠ 0 ? estim.C*Z̃[begin]^2 : zero(NT) - #println(x̂0arr) - #println(invP̄) - #println(dot(x̄, invP̄, x̄)) - #println(dot(Ŵ, invQ̂_Nk, Ŵ)) - #println(dot(V̂, invR̂_Nk, V̂)) return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + Jϵ end From 9d0b0ce3f85d7ec04b70266be207074a2493c2a4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 18 Mar 2025 17:27:11 -0400 Subject: [PATCH 29/30] doc: MHE exetended help details on AD backends --- src/estimator/mhe/construct.jl | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 45ea71b61..fcc1bd751 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -357,10 +357,12 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s, Ipopt optimizer - If `model` is a [`LinModel`](@ref), the optimization is treated as a quadratic program with a time-varying Hessian, which is generally cheaper than nonlinear programming. By default, a [`KalmanFilter`](@ref) estimates the arrival covariance (customizable). - - Else, a nonlinear program with automatic differentiation (AD) solves the optimization. - Optimizers generally benefit from exact derivatives like AD. However, the `f` and `h` - functions must be compatible with this feature. See the [`JuMP` documentation](@extref JuMP Common-mistakes-when-writing-a-user-defined-operator) - for common mistakes when writing these functions. An [`UnscentedKalmanFilter`](@ref) + - Else, a nonlinear program with dense [`ForwardDiff`](@extref ForwardDiff) automatic + differentiation (AD) compute the objective and constraint derivatives by default + (customizable). Optimizers generally benefit from exact derivatives like AD. However, + the `f` and `h` functions must be compatible with this feature. See the + [`JuMP` documentation](@extref JuMP Common-mistakes-when-writing-a-user-defined-operator) + for common mistakes when writing these functions. Also, an [`UnscentedKalmanFilter`](@ref) estimates the arrival covariance by default. The slack variable ``ϵ`` relaxes the constraints if enabled, see [`setconstraint!`](@ref). From 990f9c57e1260e62970f9c6b5c1cc7d4927fca0c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 20 Mar 2025 11:47:12 -0400 Subject: [PATCH 30/30] changed: avoid non-`AbstractArray`s in `DI.Constant()` By using a closure on `mpc::NonLinMPC` and `estim::MovingHorizonEstimator` --- src/controller/nonlinmpc.jl | 9 +++------ src/estimator/mhe/construct.jl | 6 ++---- 2 files changed, 5 insertions(+), 10 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 1edfd33b3..a60321367 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -600,13 +600,12 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT end return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T end - function Jfunc!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) + function Jfunc!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) end Z̃_∇J = fill(myNaN, nZ̃) ∇J_context = ( - Constant(mpc), Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(X̂0), Cache(gc), Cache(g), Cache(geq), @@ -638,12 +637,11 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT end gfuncs[i] = gfunc_i end - function gfunc!(g, Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, geq) + function gfunc!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, geq) return update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) end Z̃_∇g = fill(myNaN, nZ̃) ∇g_context = ( - Constant(mpc), Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(X̂0), Cache(gc), Cache(geq), @@ -686,12 +684,11 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT end geqfuncs[i] = geqfunc_i end - function geqfunc!(geq, Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g) + function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g) return update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) end Z̃_∇geq = fill(myNaN, nZ̃) ∇geq_context = ( - Constant(mpc), Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(X̂0), Cache(gc), Cache(g) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index fcc1bd751..b6ae2dbbb 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1346,13 +1346,12 @@ function get_optim_functions( end return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T end - function Jfunc!(Z̃, estim, V̂, X̂0, û0, ŷ0, g, x̄) + function Jfunc!(Z̃, V̂, X̂0, û0, ŷ0, g, x̄) update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃) return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) end Z̃_∇J = fill(myNaN, nZ̃) ∇J_context = ( - Constant(estim), Cache(V̂), Cache(X̂0), Cache(û0), Cache(ŷ0), Cache(g), @@ -1383,12 +1382,11 @@ function get_optim_functions( end gfuncs[i] = gfunc_i end - function gfunc!(g, Z̃, estim, V̂, X̂0, û0, ŷ0) + function gfunc!(g, Z̃, V̂, X̂0, û0, ŷ0) return update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃) end Z̃_∇g = fill(myNaN, nZ̃) ∇g_context = ( - Constant(estim), Cache(V̂), Cache(X̂0), Cache(û0), Cache(ŷ0), )