Merge pull request #290 from JuliaControl/getinfo_deriv

added: derivatives in `getinfo` dictionnary for `NonLinMPC` and `MovingHorizonEstimator`

Author: franckgaga

Project.toml

@@ -1,6 +1,6 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
-version = "1.13.3"
+version = "1.14.0"
 authors = ["Francis Gagnon"]
 
 [deps]

src/ModelPredictiveControl.jl

@@ -9,6 +9,8 @@ using RecipesBase
 
 using DifferentiationInterface: ADTypes.AbstractADType, AutoForwardDiff
 using DifferentiationInterface: AutoSparse, SecondOrder
+using DifferentiationInterface: gradient, jacobian, hessian
+using DifferentiationInterface: value_and_gradient, value_gradient_and_hessian
 using DifferentiationInterface: gradient!, value_and_gradient!, prepare_gradient
 using DifferentiationInterface: jacobian!, value_and_jacobian!, prepare_jacobian 
 using DifferentiationInterface: hessian!, value_gradient_and_hessian!, prepare_hessian

src/controller/execute.jl

@@ -105,8 +105,20 @@ The function should be called after calling [`moveinput!`](@ref). It returns the
 - `:d`   : current measured disturbance, ``\mathbf{d}(k)``
 
 For [`LinMPC`](@ref) and [`NonLinMPC`](@ref), the field `:sol` also contains the optimizer
-solution summary that can be printed. Lastly, the economical cost `:JE` and the custom
-nonlinear constraints `:gc` values at the optimum are also available for [`NonLinMPC`](@ref).
+solution summary that can be printed. Lastly, for [`NonLinMPC`](@ref), the following fields
+are also available:
+
+- `:JE`: economic cost value at the optimum, ``J_E``
+- `:gc`: custom nonlinear constraints values at the optimum, ``\mathbf{g_c}``
+- `:∇J` or *`:nablaJ`* : gradient of the objective function, ``\mathbf{\nabla} J``
+- `:∇²J` or *`:nabla2J`* : Hessian of the objective function, ``\mathbf{\nabla^2}J``
+- `:∇g` or *`:nablag`* : Jacobian of the inequality constraint, ``\mathbf{\nabla g}``
+- `:∇²ℓg` or *`:nabla2lg`* : Hessian of the inequality Lagrangian, ``\mathbf{\nabla^2}\ell_{\mathbf{g}}``
+- `:∇geq` or *`:nablageq`* : Jacobian of the equality constraint, ``\mathbf{\nabla g_{eq}}``
+- `:∇²ℓgeq` or *`:nabla2lgeq`* : Hessian of the equality Lagrangian, ``\mathbf{\nabla^2}\ell_{\mathbf{g_{eq}}}``
+
+Note that Hessian of Lagrangians are not fully supported yet. Their nonzero coefficients are
+random values for now.
 
 # Examples
 ```jldoctest

src/controller/nonlinmpc.jl

@@ -526,9 +526,11 @@ end
 """
     addinfo!(info, mpc::NonLinMPC) -> info
 
-For [`NonLinMPC`](@ref), add `:sol` and the optimal economic cost `:JE`.
+For [`NonLinMPC`](@ref), add `:sol`, the custom nonlinear objective `:JE`, the custom
+constraint `:gc`, and the various derivatives.
 """
 function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real
+    # --- variables specific to NonLinMPC ---
     U, Ŷ, D̂, ŷ, d, ϵ = info[:U], info[:Ŷ], info[:D̂], info[:ŷ], info[:d], info[:ϵ]
     Ue = [U; U[(end - mpc.estim.model.nu + 1):end]]
     Ŷe = [ŷ; Ŷ]
@@ -539,6 +541,118 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real
     info[:JE]  = JE 
     info[:gc] = LHS
     info[:sol] = JuMP.solution_summary(mpc.optim, verbose=true)
+    # --- objective derivatives ---
+    model, optim, con = mpc.estim.model, mpc.optim, mpc.con
+    transcription = mpc.transcription
+    nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ
+    nk = get_nk(model, transcription)
+    Hp, Hc = mpc.Hp, mpc.Hc
+    ng = length(con.i_g)
+    nc, neq = con.nc, con.neq
+    nU, nŶ, nX̂, nK = mpc.Hp*nu, Hp*ny, Hp*nx̂, Hp*nk
+    nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny  
+    ΔŨ     = zeros(NT, nΔŨ)
+    x̂0end  = zeros(NT, nx̂)
+    K0     = zeros(NT, nK)
+    Ue, Ŷe = zeros(NT, nUe), zeros(NT, nŶe)
+    U0, Ŷ0 = zeros(NT, nU),  zeros(NT, nŶ)
+    Û0, X̂0 = zeros(NT, nU),  zeros(NT, nX̂)
+    gc, g  = zeros(NT, nc),  zeros(NT, ng)
+    geq    = zeros(NT, neq)
+    J_cache = (
+        Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), 
+        Cache(Û0), Cache(K0), Cache(X̂0), 
+        Cache(gc), Cache(g), Cache(geq),
+    )
+    function J!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq)
+        update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃)
+        return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)
+    end
+    if !isnothing(mpc.hessian)
+        _, ∇J, ∇²J = value_gradient_and_hessian(J!, mpc.hessian, mpc.Z̃, J_cache...)
+    else
+        ∇J, ∇²J = gradient(J!, mpc.gradient, mpc.Z̃, J_cache...), nothing
+    end
+    # --- inequality constraint derivatives ---
+    old_i_g = copy(con.i_g)
+    con.i_g .= 1 # temporarily set all constraints as finite so g is entirely computed
+    ∇g_cache = (
+        Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), 
+        Cache(Û0), Cache(K0), Cache(X̂0), 
+        Cache(gc), Cache(geq),
+    )
+    function g!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq)
+        update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃)
+        return nothing
+    end
+    ∇g = jacobian(g!, g, mpc.jacobian, mpc.Z̃, ∇g_cache...)
+    if !isnothing(mpc.hessian) && any(old_i_g)
+        @warn(
+            "Retrieving optimal Hessian of the Lagrangian is not fully supported yet.\n"*
+            "Its nonzero coefficients are random values for now.", maxlog=1
+        )
+        function ℓ_g(Z̃, λ, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g)
+            update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃)
+            return dot(λ, g)
+        end
+        ∇²g_cache = (
+            Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), 
+            Cache(Û0), Cache(K0), Cache(X̂0), 
+            Cache(gc), Cache(geq), Cache(g)
+        )
+        nonlincon = optim[:nonlinconstraint]
+        λ = JuMP.dual.(nonlincon) # FIXME: does not work for now
+        λ = rand(NT, ng)
+        ∇²ℓg = hessian(ℓ_g, mpc.hessian, mpc.Z̃, Constant(λ), ∇²g_cache...)
+    else
+        ∇²ℓg = nothing
+    end
+    con.i_g .= old_i_g # restore original finite/infinite constraint indices
+    # --- equality constraint derivatives ---
+    geq_cache = (
+        Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0),
+        Cache(Û0), Cache(K0),   Cache(X̂0),
+        Cache(gc), Cache(g)
+    )
+    function geq!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g) 
+        update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃)
+        return nothing
+    end
+    ∇geq = jacobian(geq!, geq, mpc.jacobian, mpc.Z̃, geq_cache...)
+    if !isnothing(mpc.hessian) && con.neq > 0
+        @warn(
+            "Retrieving optimal Hessian of the Lagrangian is not fully supported yet.\n"*
+            "Its nonzero coefficients are random values for now.", maxlog=1
+        )
+        ∇²geq_cache = (
+            Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0),
+            Cache(Û0), Cache(K0),   Cache(X̂0),
+            Cache(gc), Cache(geq), Cache(g)
+        )
+        function ℓ_geq(Z̃, λeq, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g)
+            update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃)
+            return dot(λeq, geq)
+        end
+        nonlinconeq = optim[:nonlinconstrainteq]
+        λeq = JuMP.dual.(nonlinconeq) # FIXME: does not work for now
+        λeq = ones(NT, neq)
+        ∇²ℓgeq = hessian(ℓ_geq, mpc.hessian, mpc.Z̃, Constant(λeq), ∇²geq_cache...)
+    else
+        ∇²ℓgeq = nothing
+    end
+    info[:∇J] = ∇J
+    info[:∇²J] = ∇²J
+    info[:∇g] = ∇g
+    info[:∇²ℓg] = ∇²ℓg
+    info[:∇geq] = ∇geq
+    info[:∇²ℓgeq] = ∇²ℓgeq
+    # --- non-Unicode fields ---
+    info[:nablaJ] = ∇J
+    info[:nabla2J] = ∇²J
+    info[:nablag] = ∇g
+    info[:nabla2lg] = ∇²ℓg
+    info[:nablageq] = ∇geq
+    info[:nabla2lgeq] = ∇²ℓgeq
     return info
 end
 
@@ -942,10 +1056,10 @@ function set_nonlincon!(
         optim, JuMP.Vector{JuMP.VariableRef}, MOI.VectorNonlinearOracle{JNT}
     )
     map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints)
-    optim[:g_oracle]   = g_oracle
-    optim[:geq_oracle] = geq_oracle
-    any(mpc.con.i_g) && @constraint(optim, Z̃var in g_oracle)
-    mpc.con.neq > 0  && @constraint(optim, Z̃var in geq_oracle)
+    JuMP.unregister(optim, :nonlinconstraint)
+    JuMP.unregister(optim, :nonlinconstrainteq)
+    any(mpc.con.i_g) && @constraint(optim, nonlinconstraint, Z̃var in g_oracle)
+    mpc.con.neq > 0  && @constraint(optim, nonlinconstrainteq, Z̃var in geq_oracle)
     return nothing
 end
 

src/estimator/mhe/construct.jl

@@ -1504,9 +1504,7 @@ function get_nonlincon_oracle(
         return dot(λi, gi)
     end
     Z̃_∇gi = fill(myNaN, nZ̃)      # NaN to force update_predictions! at first call
-    ∇gi_cache = (
-        Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g)
-    )
+    ∇gi_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g))
     # temporarily "fill" the estimation window for the preparation of the gradient: 
     estim.Nk[] = He
     ∇gi_prep = prepare_jacobian(gi!, gi, jac, Z̃_∇gi, ∇gi_cache...; strict)
@@ -1577,7 +1575,7 @@ function set_nonlincon!(
         optim, JuMP.Vector{JuMP.VariableRef}, MOI.VectorNonlinearOracle{JNT}
     )
     map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints)
-    optim[:g_oracle]   = g_oracle
-    any(estim.con.i_g) && @constraint(optim, Z̃var in g_oracle)
+    JuMP.unregister(optim, :nonlinconstraint)
+    any(estim.con.i_g) && @constraint(optim, nonlinconstraint, Z̃var in g_oracle)
     return nothing
 end

src/estimator/mhe/execute.jl

@@ -94,6 +94,18 @@ following fields:
 - `:D`   : measured disturbances over ``N_k``, ``\mathbf{D}``
 - `:sol` : solution summary of the optimizer for printing
 
+For [`NonLinModel`](@ref), it also includes the following derivative fields:
+
+- `:JE`: economic cost value at the optimum, ``J_E``
+- `:gc`: custom nonlinear constraints values at the optimum, ``\mathbf{g_c}``
+- `:∇J` or *`:nablaJ`* : gradient of the objective function, ``\mathbf{\nabla} J``
+- `:∇²J` or *`:nabla2J`* : Hessian of the objective function, ``\mathbf{\nabla^2}J``
+- `:∇g` or *`:nablag`* : Jacobian of the inequality constraint, ``\mathbf{\nabla g}``
+- `:∇²ℓg` or *`:nabla2lg`* : Hessian of the inequality Lagrangian, ``\mathbf{\nabla^2}\ell_{\mathbf{g}}``
+
+Note that Hessian of Lagrangians are not fully supported yet. Their nonzero coefficients are
+random values for now.
+
 # Examples
 ```jldoctest
 julia> model = LinModel(ss(1.0, 1.0, 1.0, 0, 5.0));
@@ -163,9 +175,85 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real
     info[:Yhatm] = info[:Ŷm]
     # --- deprecated fields ---
     info[:ϵ] = info[:ε]
+    info = addinfo!(info, estim, model)
     return info
 end
 
+
+"""
+    addinfo!(info, estim::MovingHorizonEstimator, model::NonLinModel)
+
+For [`NonLinModel`](@ref), add the various derivatives.
+"""
+function addinfo!(
+    info, estim::MovingHorizonEstimator{NT}, model::NonLinModel
+) where NT <:Real
+    # --- objective derivatives ---
+    optim, con = estim.optim, estim.con
+    nx̂, nym, nŷ, nu, nk = estim.nx̂, estim.nym, model.ny, model.nu, model.nk
+    He = estim.He
+    nV̂, nX̂, ng = He*nym, He*nx̂, length(con.i_g)
+    V̂,  X̂0 = zeros(NT, nV̂), zeros(NT, nX̂)
+    k0     = zeros(NT, nk)
+    û0, ŷ0 = zeros(NT, nu), zeros(NT, nŷ)
+    g      = zeros(NT, ng) 
+    x̄      = zeros(NT, nx̂)
+    J_cache = (
+        Cache(V̂),  Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0),
+        Cache(g),
+        Cache(x̄),
+    )
+    function J!(Z̃, V̂, X̂0, û0, k0, ŷ0, g, x̄)
+        update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃)
+        return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)
+    end
+    if !isnothing(estim.hessian)
+        _, ∇J, ∇²J = value_gradient_and_hessian(J!, estim.hessian, estim.Z̃, J_cache...)
+    else
+        ∇J, ∇²J = gradient(J!, estim.gradient, estim.Z̃, J_cache...), nothing
+    end
+    # --- inequality constraint derivatives ---
+    old_i_g = copy(estim.con.i_g)
+    estim.con.i_g .= 1 # temporarily set all constraints as finite so g is entirely computed
+    ∇g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0))
+    function g!(g, Z̃, V̂, X̂0, û0, k0, ŷ0)
+        update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃)
+        return nothing
+    end
+    ∇g = jacobian(g!, g, estim.jacobian, estim.Z̃, ∇g_cache...)
+    if !isnothing(estim.hessian) && any(old_i_g)
+        @warn(
+            "Retrieving optimal Hessian of the Lagrangian is not fully supported yet.\n"*
+            "Its nonzero coefficients are random values for now.", maxlog=1
+        )
+        ∇²g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g))
+        function ℓ_g(Z̃, λ, V̂, X̂0, û0, k0, ŷ0, g)
+            update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃)
+            return dot(λ, g)
+        end
+        nonlincon = optim[:nonlinconstraint]
+        λ = JuMP.dual.(nonlincon) # FIXME: does not work for now
+        λ = ones(NT, ng)
+        ∇²ℓg = hessian(ℓ_g, estim.hessian, estim.Z̃, Constant(λ), ∇²g_cache...)
+    else
+        ∇²ℓg = nothing
+    end
+    estim.con.i_g .= old_i_g # restore original finite/infinite constraint indices
+    info[:∇J] = ∇J
+    info[:∇²J] = ∇²J
+    info[:∇g] = ∇g
+    info[:∇²ℓg] = ∇²ℓg
+    # --- non-Unicode fields ---
+    info[:nablaJ] = ∇J
+    info[:nabla2J] = ∇²J
+    info[:nablag] = ∇g
+    info[:nabla2lg] = ∇²ℓg
+    return info
+end
+
+"Nothing to add in the `info` dict for [`LinModel`](@ref)."
+addinfo!(info, ::MovingHorizonEstimator, ::LinModel) = info
+
 """
     getε(estim::MovingHorizonEstimator, Z̃) -> ε
 

src/general.jl

@@ -16,6 +16,16 @@ const ALL_COLORING_ORDERS = (
     RandomOrder(StableRNG(0), 0)
 )
 
+const HIDDEN_GETINFO_KEYS_MHE = (
+    :What, :xhatarr, :epsilon, :Xhat, :xhat, :Vhat, :Pbar, :xbar, :Yhat, :Yhatm, :ϵ,
+    :nablaJ, :nabla2J, :nablag, :nabla2lg
+)
+
+const HIDDEN_GETINFO_KEYS_MPC = (
+    :DeltaU, :epsilon, :Dhat, :yhat, :Yhat, :xhatend, :Yhats, :Rhaty, :Rhatu,
+    :nablaJ, :nabla2J, :nablag, :nabla2lg, :nablageq, :nabla2lgeq
+)
+
 "Termination status that means 'no solution available'."
 const ERROR_STATUSES = (
     JuMP.INFEASIBLE, JuMP.DUAL_INFEASIBLE, JuMP.LOCALLY_INFEASIBLE, 
@@ -40,12 +50,17 @@ function info2debugstr(info)
     mystr = "Content of getinfo dictionary:\n"
     for (key, value) in info
         (key == :sol) && continue
+        if key in HIDDEN_GETINFO_KEYS_MHE || key in HIDDEN_GETINFO_KEYS_MPC
+            # skip the redundant non-Unicode keys
+            continue
+        end
         mystr *= "  :$key => $value\n"
     end
     if haskey(info, :sol)
         split_sol = split(string(info[:sol]), "\n")
-        solstr = join((lpad(line, length(line) + 2) for line in split_sol), "\n", "")
-        mystr *= "  :sol => \n"*solstr
+        # Add the treeview prefix to each line
+        solstr = join(("   " * line for line in split_sol), "\n")
+        mystr *= "  :sol => \n" * solstr * "\n"  # Ensure a trailing newline
     end
     return mystr
 end

test/2_test_state_estim.jl

@@ -967,6 +967,7 @@ end
     @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
 
     mhe1c = MovingHorizonEstimator(nonlinmodel, He=2, direct=false, hessian=true)
+    mhe1c = setconstraint!(mhe1c, v̂min = [-1000, -1000], v̂max = [1000, 1000]) # coverage of getinfo Hessian of Lagrangian
     preparestate!(mhe1c, [50, 30], [5])
     x̂ = updatestate!(mhe1c, [10, 50], [50, 30], [5])
     @test x̂ ≈ zeros(6) atol=1e-9

test/3_test_predictive_control.jl

@@ -814,7 +814,6 @@ end
     preparestate!(nmpc4, [0], [0])
     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])
     f! = (ẋ,x,u,_,_) -> ẋ .= -0.001x .+ u 
@@ -830,6 +829,7 @@ end
     preparestate!(nmpc5_1, [0.0])
     u = moveinput!(nmpc5_1, [1/0.001])
     @test u ≈ [1.0] atol=5e-2
+    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])
     @test moveinput!(nmpc6, [0]) ≈ [0.0] atol=5e-2
@@ -848,7 +848,8 @@ end
     @test info[:u] ≈ u
     @test info[:Ŷ][end] ≈ 10 atol=5e-2
     transcription = MultipleShooting(f_threads=true, h_threads=true)
-    nmpc8t = NonLinMPC(nonlinmodel; Nwt=[0], Hp=100, Hc=1, transcription)
+    nmpc8t = NonLinMPC(nonlinmodel; Nwt=[0], Hp=100, Hc=1, transcription, hessian=true)
+    nmpc8t = setconstraint!(nmpc8t, ymax=[100], ymin=[-100]) # coverage of getinfo! Hessians of Lagrangian
     preparestate!(nmpc8t, [0], [0])
     u = moveinput!(nmpc8t, [10], [0])
     @test u ≈ [2] atol=5e-2