added: NonLinMPC and MovingHorizonEstimator integration with DI.jl

Project.toml

@@ -1,51 +1,51 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "1.4.4"
+version = "1.5.0"
 
 [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"
+SparseConnectivityTracer = "9f842d2f-2579-4b1d-911e-f412cf18a3f5"
+SparseMatrixColorings = "0a514795-09f3-496d-8182-132a7b665d35"
 
 [compat]
-julia = "1.10"
-LinearAlgebra = "1.10"
-Logging = "1.10"
-Random = "1.10"
 ControlSystemsBase = "1.9"
+DifferentiationInterface = "0.6.45"
 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"
+SparseConnectivityTracer = "0.6.13"
+SparseMatrixColorings = "0.4.14"
+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"
-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", "FiniteDiff"]

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

docs/make.jl

@@ -3,9 +3,17 @@ 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",
+    "ForwardDiff" => "https://juliadiff.org/ForwardDiff.jl/stable/objects.inv",
+)
+
 DocMeta.setdocmeta!(
     ModelPredictiveControl, 
     :DocTestSetup, 
@@ -16,6 +24,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"

docs/src/index.md

@@ -4,14 +4,17 @@ 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
 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` 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.
 
 The documentation is divided in two parts:
 

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.

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

src/ModelPredictiveControl.jl

@@ -1,12 +1,19 @@
 module ModelPredictiveControl
 
-using PrecompileTools
+using PrecompileTools # TODO: remove this dep if possible (with Cache of DI.jl)
 using LinearAlgebra
 using Random: randn
 
 using RecipesBase
 using ProgressLogging
-using ForwardDiff
+
+using DifferentiationInterface: ADTypes.AbstractADType, AutoForwardDiff, AutoSparse
+using DifferentiationInterface: gradient!, jacobian!, prepare_gradient, prepare_jacobian
+using DifferentiationInterface: Constant, Cache
+using SparseConnectivityTracer: TracerSparsityDetector
+using SparseMatrixColorings: GreedyColoringAlgorithm, sparsity_pattern
+
+import ForwardDiff #TODO: delete this after `linearize!` and `ExtendedKalmanFilter` are updated
 
 import ControlSystemsBase
 import ControlSystemsBase: ss, tf, delay

src/controller/execute.jl

@@ -123,19 +123,19 @@ 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[:ϵ]    = 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
 
@@ -361,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)

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