debug: force derivatives for first NLP iteration

Project.toml

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

src/controller/execute.jl

@@ -467,6 +467,7 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real}
     model, optim = mpc.estim.model, mpc.optim
     Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var]
     Z̃s = set_warmstart_mpc!(mpc, mpc.transcription, Z̃var)
+    set_force∇!(mpc)
     set_objective_linear_coef!(mpc, model, Z̃var)
     try
         JuMP.optimize!(optim)
@@ -504,6 +505,8 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real}
     return mpc.Z̃
 end
 
+"By default, no need to force update the derivatives."
+set_force∇!(::PredictiveController) = nothing
 
 "By default, no need to update the objective function."
 set_objective_linear_coef!(::PredictiveController, ::SimModelODE, _) = nothing
@@ -514,7 +517,6 @@ function set_objective_linear_coef!(mpc::PredictiveController, ::LinModel, Z̃va
     return nothing
 end
 
-
 """
     preparestate!(mpc::PredictiveController, ym, d=[]) -> x̂
 

src/controller/nonlinmpc.jl

@@ -27,6 +27,9 @@ struct NonLinMPC{
     gradient::GB
     jacobian::JB
     hessian::HB
+    force∇J::Vector{Bool}
+    force∇g::Vector{Bool}
+    force∇geq::Vector{Bool}
     Z̃::Vector{NT}
     ŷ::Vector{NT}
     ry::Vector{NT}
@@ -123,10 +126,13 @@ struct NonLinMPC{
         test_custom_function_mpc(NT, model, JE, gc!, nc, Uop, Yop, Dop, p)
         nZ̃ = get_nZ_mpc(estim, transcription, Hp, Hc) + nϵ
         Z̃ = zeros(NT, nZ̃)
+        # force computation of derivatives for the first NLP iteration:
+        force∇J, force∇g, force∇geq = [true], [true], [true]
         buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ)
         mpc = new{NT, SE, CW, TM, JM, GB, JB, HB, PT, JEfunc, GCfunc}(
             estim, transcription, optim, con,
             gradient, jacobian, hessian,
+            force∇J, force∇g, force∇geq,
             Z̃, ŷ, ry,
             Hp, Hc, nϵ, nb,
             weights,
@@ -830,7 +836,6 @@ function get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where J
     nZ̃, nU, nŶ, nX̂, nK̄ = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nk̄
     nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny  
     strict = Val(true)
-    myNaN                            = convert(JNT, NaN)
     J::Vector{JNT}                   = zeros(JNT, 1)
     ΔŨ::Vector{JNT}                  = zeros(JNT, nΔŨ)
     x̂0end::Vector{JNT}               = zeros(JNT, nx̂)
@@ -844,7 +849,7 @@ function get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where J
         update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃)
         return obj_nonlinprog!(Ŷ0, U0, mpc, Ue, Ŷe, ΔŨ)
     end
-    Z̃_J = fill(myNaN, nZ̃)      # NaN to force update at first call
+    Z̃_J = zeros(JNT, nZ̃)
     J_cache = (
         Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), 
         Cache(Û0), Cache(K̄), Cache(X̂0), 
@@ -859,19 +864,21 @@ function get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where J
     end
     update_objective! = if !isnothing(hess)
         function (J, ∇J, ∇²J, Z̃_J, Z̃_arg)
-            if isdifferent(Z̃_arg, Z̃_J)
+            if isdifferent(Z̃_arg, Z̃_J) || mpc.force∇J[]
+                mpc.force∇J[] = false
                 Z̃_J .= Z̃_arg
                 J[], _ = value_gradient_and_hessian!(
                     J!, ∇J, ∇²J, ∇²J_prep, hess, Z̃_J, J_cache...
                 )
             end
         end
     else
-        function (J, ∇J, Z̃_∇J, Z̃_arg)
-            if isdifferent(Z̃_arg, Z̃_∇J)
-                Z̃_∇J .= Z̃_arg
+        function (J, ∇J, Z̃_J, Z̃_arg)
+            if isdifferent(Z̃_arg, Z̃_J) || mpc.force∇J[]
+                mpc.force∇J[] = false
+                Z̃_J .= Z̃_arg
                 J[], _ = value_and_gradient!(
-                    J!, ∇J, ∇J_prep, grad, Z̃_∇J, J_cache...
+                    J!, ∇J, ∇J_prep, grad, Z̃_J, J_cache...
                 )
             end
         end
@@ -957,7 +964,7 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN
     nZ̃, nU, nŶ, nX̂, nK̄ = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nk̄
     nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny  
     strict = Val(true)
-    myNaN, myInf                      = convert(JNT, NaN), convert(JNT, Inf)
+    myInf                             = convert(JNT, Inf)
     ΔŨ::Vector{JNT}                   = zeros(JNT, nΔŨ)
     x̂0end::Vector{JNT}                = zeros(JNT, nx̂)
     K̄::Vector{JNT}                    = zeros(JNT, nK̄)
@@ -978,7 +985,7 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN
         gi .= @views g[i_g]
         return dot(λi, gi)
     end
-    Z̃_∇gi  = fill(myNaN, nZ̃)      # NaN to force update at first call
+    Z̃_∇gi = zeros(JNT, nZ̃)
     ∇gi_cache = (
         Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), 
         Cache(Û0), Cache(K̄), Cache(X̂0), 
@@ -1000,7 +1007,8 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN
         ∇²gi_structure = lowertriangle_indices(init_diffstructure(∇²ℓ_gi))
     end
     function update_con!(gi, ∇gi, Z̃_∇gi, Z̃_arg)
-        if isdifferent(Z̃_arg, Z̃_∇gi)
+        if isdifferent(Z̃_arg, Z̃_∇gi) || mpc.force∇g[]
+            mpc.force∇g[] = false
             Z̃_∇gi .= Z̃_arg
             value_and_jacobian!(gi!, gi, ∇gi, ∇gi_prep, jac, Z̃_∇gi, ∇gi_cache...)
         end
@@ -1041,7 +1049,7 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN
         update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc, Z̃)
         return dot(λeq, geq)
     end
-    Z̃_∇geq = fill(myNaN, nZ̃)    # NaN to force update at first call
+    Z̃_∇geq = zeros(JNT, nZ̃)
     ∇geq_cache = (
         Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0),
         Cache(Û0), Cache(K̄),   Cache(X̂0),
@@ -1063,7 +1071,8 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN
         ∇²geq_structure = lowertriangle_indices(init_diffstructure(∇²ℓ_geq))
     end
     function update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃_arg)
-        if isdifferent(Z̃_arg, Z̃_∇geq)
+        if isdifferent(Z̃_arg, Z̃_∇geq) || mpc.force∇geq[]
+            mpc.force∇geq[] = false
             Z̃_∇geq .= Z̃_arg
             value_and_jacobian!(geq!, geq, ∇geq, ∇geq_prep, jac, Z̃_∇geq, ∇geq_cache...)
         end
@@ -1097,6 +1106,14 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN
     return g_oracle, geq_oracle
 end
 
+"Force the computation of the derivatives for the first NLP iteration."
+function set_force∇!(mpc::NonLinMPC)
+    mpc.force∇J[] = true
+    mpc.force∇g[] = true
+    mpc.force∇geq[] = true
+    return nothing
+end
+
 """
     update_predictions!(
         ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, 

src/estimator/mhe/construct.jl

@@ -93,6 +93,9 @@ struct MovingHorizonEstimator{
     gradient::GB
     jacobian::JB
     hessian::HB
+    force∇J::Vector{Bool}
+    force∇g::Vector{Bool}
+    force∇geq::Vector{Bool}
     cov::KC
     covestim::CE
     Z̃::Vector{NT}
@@ -216,10 +219,13 @@ struct MovingHorizonEstimator{
         Nk = [0]
         prepared = [false]
         test_custom_function_mhe(NT, model, i_ym, He, gc!, nc, x̂op, p, direct)
+        # force computation of derivatives for the first NLP iteration:
+        force∇J, force∇g, force∇geq = [true], [true], [true]
         buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk̄, He, nŵ, nε, transcription)
         estim = new{NT, SM, KC, TM, JM, GB, JB, HB, PT, GCfunc, CE}(
             model, transcription, optim, con, 
             gradient, jacobian, hessian,
+            force∇J, force∇g, force∇geq,
             cov,
             covestim,  
             Z̃, lastu0, x̂op, f̂op, x̂0, 
@@ -1433,7 +1439,6 @@ function get_nonlinobj_op(
     nK̄, nU, nŶ = He*nk̄, He*nu, He*nŷ
     nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym
     strict = Val(true)
-    myNaN                               = convert(JNT, NaN)
     J::Vector{JNT}                      = zeros(JNT, 1)
     x̂0arr::Vector{JNT}, x̄::Vector{JNT}  = zeros(JNT, nx̂),  zeros(JNT, nx̂)
     Ŵ::Vector{JNT}                      = zeros(JNT, nŴ)
@@ -1450,7 +1455,7 @@ function get_nonlinobj_op(
         )
         return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃)
     end
-    Z̃_J = fill(myNaN, nZ̃)      # NaN to force update_predictions! at first call
+    Z̃_J = zeros(JNT, nZ̃)
     J_cache = (
         Cache(x̂0arr), Cache(x̄), 
         Cache(Ŵ), Cache(V̂), Cache(X̂0), 
@@ -1471,18 +1476,20 @@ function get_nonlinobj_op(
         ∇²J_structure = lowertriangle_indices(init_diffstructure(∇²J))
     end
     update_objective! = if !isnothing(hess)
-        function (J, ∇J, ∇²J, Z̃_∇J, Z̃_arg)
-            if isdifferent(Z̃_arg, Z̃_∇J)
-                Z̃_∇J .= Z̃_arg
+        function (J, ∇J, ∇²J, Z̃_J, Z̃_arg)
+            if isdifferent(Z̃_arg, Z̃_J) || estim.force∇J[]
+                estim.force∇J[] = false
+                Z̃_J .= Z̃_arg
                 J[], _ = value_gradient_and_hessian!(
                     J!, ∇J, ∇²J, ∇²J_prep, hess, Z̃_J, J_cache...
                 )
             end
         end    
     else
-        function (J, ∇J, Z̃_∇J, Z̃_arg)
-            if isdifferent(Z̃_arg, Z̃_∇J)
-                Z̃_∇J .= Z̃_arg
+        function (J, ∇J, Z̃_J, Z̃_arg)
+            if isdifferent(Z̃_arg, Z̃_J) || estim.force∇J[]
+                estim.force∇J[] = false
+                Z̃_J .= Z̃_arg
                 J[], _ = value_and_gradient!(J!, ∇J, ∇J_prep, grad, Z̃_J, J_cache...)
             end
         end
@@ -1551,7 +1558,7 @@ function get_nonlincon_oracle(
     nK̄, nU, nŶ = He*nk̄, He*nu, He*nŷ
     nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym
     strict = Val(true)
-    myNaN, myInf                          = convert(JNT, NaN), convert(JNT, Inf)
+    myInf                                 = convert(JNT, Inf)
     x̂0arr::Vector{JNT}, x̄::Vector{JNT}    = zeros(JNT, nx̂),  zeros(JNT, nx̂)
     Ŵ::Vector{JNT}                        = zeros(JNT, nŴ)
     V̂::Vector{JNT},     X̂0::Vector{JNT}   = zeros(JNT, nV̂),  zeros(JNT, nX̂)
@@ -1578,7 +1585,7 @@ function get_nonlincon_oracle(
         gi .= @views g[i_g]
         return dot(λi, gi)
     end
-    Z̃_∇gi = fill(myNaN, nZ̃)      # NaN to force update_predictions! at first call
+    Z̃_∇gi = zeros(JNT, nZ̃)
     ∇gi_cache = (
         Cache(x̂0arr), Cache(x̄), 
         Cache(Ŵ), Cache(V̂), Cache(X̂0), 
@@ -1610,7 +1617,8 @@ function get_nonlincon_oracle(
         ∇²gi_structure = lowertriangle_indices(init_diffstructure(∇²ℓ_gi))
     end
     function update_con!(gi, ∇gi, Z̃_∇gi, Z̃_arg)
-        if isdifferent(Z̃_arg, Z̃_∇gi)
+        if isdifferent(Z̃_arg, Z̃_∇gi) || estim.force∇g[]
+            estim.force∇g[] = false
             Z̃_∇gi .= Z̃_arg
             value_and_jacobian!(gi!, gi, ∇gi, ∇gi_prep, jac, Z̃_∇gi, ∇gi_cache...)
         end
@@ -1655,7 +1663,7 @@ function get_nonlincon_oracle(
         )
         return dot(λeq, geq)
     end
-    Z̃_∇geq = fill(myNaN, nZ̃)    # NaN to force update at first call
+    Z̃_∇geq = zeros(JNT, nZ̃)
     ∇geq_cache = (
         Cache(x̂0arr), Cache(x̄), 
         Cache(Ŵ), Cache(V̂), Cache(X̂0), 
@@ -1685,7 +1693,8 @@ function get_nonlincon_oracle(
         ∇²geq_structure = lowertriangle_indices(init_diffstructure(∇²ℓ_geq))
     end
     function update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃_arg)
-        if isdifferent(Z̃_arg, Z̃_∇geq)
+        if isdifferent(Z̃_arg, Z̃_∇geq) || estim.force∇geq[]
+            estim.force∇geq[] = false
             Z̃_∇geq .= Z̃_arg
             value_and_jacobian!(geq!, geq, ∇geq, ∇geq_prep, jac, Z̃_∇geq, ∇geq_cache...)
         end

src/estimator/mhe/execute.jl

@@ -585,6 +585,7 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real
     optim = estim.optim
     Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var]
     Z̃s = set_warmstart_mhe!(estim, estim.transcription, Z̃var)
+    set_force∇!(estim)
     # ------- solve optimization problem --------------
     try
         JuMP.optimize!(optim)
@@ -624,6 +625,14 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real
     return estim.Z̃
 end
 
+"Force the computation of the derivatives for the first NLP iteration (if applicable)."
+function set_force∇!(estim::MovingHorizonEstimator)
+    estim.force∇J[] = true # the fields are ignored if QP instead of NLP 
+    estim.force∇g[] = true
+    estim.force∇geq[] = true
+    return nothing
+end
+
 @doc raw"""
     getstate!(estim::MovingHorizonEstimator, Z̃)
 

src/model/nonlinmodeldae.jl

@@ -20,6 +20,8 @@ struct NonLinModelDAE{
     optim_output::JMO
     jacobian::JB
     hessian::HB
+    force∇geq::Vector{Bool}
+    force∇q::Vector{Bool}
     Z::Vector{NT}
     fq!::FQ
     h!::H
@@ -96,11 +98,15 @@ struct NonLinModelDAE{
         beq = zeros(NT, size(Aeq, 1))
         neq = nZ - size(Aeq, 1) # number of nonlinear equality constraints
         x0_optim, u0_optim, d0_optim = zeros(NT, nx), zeros(NT, nu), zeros(NT, nd)
+        # force computation of derivatives for the first NLP iteration:
+        force∇q, force∇geq = [true], [true]
         buffer = SimModelBuffer{NT}(nu, nx, ny, nd, 0, na)
         model = new{NT, TM, JMS, JMO, JB, HB, FQ, H, PT}(
             x0, a0,
             transcription,
-            optim_state, optim_output, jacobian, hessian,
+            optim_state, optim_output, 
+            jacobian, hessian,
+            force∇q, force∇geq,
             Z,
             fq!, h!,
             p,
@@ -481,7 +487,6 @@ function get_nonlincon_oracle(
     nx, na, neq, nk̄ = model.nx, model.na, model.neq, get_nk̄(model, transcription)
     nZ = length(model.Z)
     strict = Val(true) 
-    myNaN                              = convert(JNT, NaN)
     k̄::Vector{JNT}                     = zeros(JNT, nk̄)
     geq::Vector{JNT}, λeq::Vector{JNT} = zeros(JNT, neq), rand(JNT, neq)
     q::Vector{JNT},   λq::Vector{JNT}  = zeros(JNT, na),  rand(JNT, na)
@@ -495,7 +500,7 @@ function get_nonlincon_oracle(
         update_predictions!(k̄, geq, model, Z)
         return dot(λeq, geq)
     end
-    Z_∇geq = fill(myNaN, nZ)    # NaN to force update at first call
+    Z_∇geq = zeros(JNT, nZ)
     ∇geq_prep = prepare_jacobian(geq!, geq, jac, Z_∇geq, Cache(k̄); strict)
     ∇geq    = init_diffmat(JNT, jac, ∇geq_prep, nZ, neq)
     ∇geq_structure  = init_diffstructure(∇geq)
@@ -507,7 +512,8 @@ function get_nonlincon_oracle(
         ∇²geq_structure = lowertriangle_indices(init_diffstructure(∇²ℓ_geq))
     end
     function update_con_eq!(geq, ∇geq, Z_∇geq, Z_arg)
-        if isdifferent(Z_arg, Z_∇geq)
+        if isdifferent(Z_arg, Z_∇geq) || model.force∇geq[]
+            model.force∇geq[] = false
             Z_∇geq .= Z_arg
             value_and_jacobian!(geq!, geq, ∇geq, ∇geq_prep, jac, Z_∇geq, Cache(k̄))
         end
@@ -548,7 +554,7 @@ function get_nonlincon_oracle(
         model.fq!(ẋ, q, model.x0_optim, a, model.u0_optim, model.d0_optim, model.p)
         return dot(λq, q)
     end
-    a_∇q = fill(myNaN, na)    # NaN to force update at first call
+    a_∇q = zeros(JNT, na)
     ∇q_prep      = prepare_jacobian(q!, q, jac, a_∇q, Cache(ẋ); strict)
     ∇q           = init_diffmat(JNT, jac, ∇q_prep, na, na)
     ∇q_structure = init_diffstructure(∇q)
@@ -558,7 +564,8 @@ function get_nonlincon_oracle(
         ∇²q_structure = lowertriangle_indices(init_diffstructure(∇²ℓ_q))
     end
     function update_con_q!(q, ∇q, a_∇q, a_arg)
-        if isdifferent(a_arg, a_∇q)
+        if isdifferent(a_arg, a_∇q) || model.force∇q[]
+            model.force∇q[] = false
             a_∇q .= a_arg
             value_and_jacobian!(q!, q, ∇q, ∇q_prep, jac, a_∇q, Cache(ẋ))
         end
@@ -726,6 +733,7 @@ Solve optimization problem `optim` with the JuMP variable `Zvar` warm-started at
 """
 function solve!(model::NonLinModelDAE, optim, Zvar, Zs)
     JuMP.set_start_value.(Zvar, Zs)
+    set_force∇!(model)
     JuMP.optimize!(optim)
     if !issolved(optim)
         status = JuMP.termination_status(optim)
@@ -748,6 +756,13 @@ function solve!(model::NonLinModelDAE, optim, Zvar, Zs)
     return Z
 end
 
+"Force the computation of the derivatives for the first NLP iteration."
+function set_force∇!(model::NonLinModelDAE)
+    model.force∇geq[] = true
+    model.force∇q[]   = true
+    return nothing
+end
+
 @doc raw"""
     getinfo(model::NonLinModelDAE) -> info