added: NonLinMPC and MovingHorizonEstimator integration with DI.jl

Author: franckgaga

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"]

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

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

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

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̃)

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,25 +669,25 @@ 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
         else
             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,19 +708,19 @@ 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̃:
         ∇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!(∇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