added: ill-conditioned QP warning at controller construction

The condition number is verified only at construction of `SingleShooting` controllers (no verification in `setmodel!`)

Author: franckgaga

docs/src/manual/nonlinmpc.md

@@ -331,7 +331,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`](@extref ForwardDiff).
+function allows automatic linearization of [`NonLinModel`](@ref) defaulting to [`ForwardDiff`](@extref ForwardDiff).
 We first linearize `model` at the point ``θ = π`` rad and ``ω = τ = 0`` (inverted position):
 
 ```@example man_nonlin
@@ -341,7 +341,6 @@ linmodel = linearize(model, x=[π, 0], u=[0])
 A [`SteadyKalmanFilter`](@ref) and a [`LinMPC`](@ref) are designed from `linmodel`:
 
 ```@example man_nonlin
-
 skf = SteadyKalmanFilter(linmodel; σQ, σR, nint_u, σQint_u)
 mpc = LinMPC(skf; Hp, Hc, Mwt, Nwt, Cwt=Inf)
 mpc = setconstraint!(mpc, umin=[-1.5], umax=[+1.5])

src/controller/construct.jl

@@ -564,7 +564,10 @@ function validate_args(mpc::PredictiveController, ry, d, D̂, R̂y, R̂u)
 end
 
 @doc raw"""
-    init_quadprog(model::LinModel, weights::ControllerWeights, Ẽ, P̃Δu, P̃u) -> H̃
+    init_quadprog(
+        model::LinModel, transcriptions::TranscriptionMethod, weights::ControllerWeights, 
+        Ẽ, P̃Δu, P̃u; warn_cond=1e6
+    ) -> H̃
 
 Init the quadratic programming Hessian `H̃` for MPC.
 
@@ -582,19 +585,43 @@ in which ``\mathbf{Ẽ}``, ``\mathbf{P̃_{Δu}}`` and ``\mathbf{P̃_{u}}`` matri
 at [`relaxŶ`](@ref), [`relaxΔU`](@ref) and [`relaxU`](@ref) documentation, respectively. The
 vector ``\mathbf{q̃}`` and scalar ``r`` need recalculation each control period ``k``, see
 [`initpred!`](@ref). ``r`` does not impact the minima position. It is thus useless at
-optimization but required to evaluate the minimal ``J`` value.
+optimization but required to evaluate the minimal ``J`` value. A `@warn` will be displayed
+if the condition number `cond(H̃) ≥ warn_cond` and `transcription` is a `SingleShooting` 
+(`warn_cond=Inf` for no warning).
 """
-function init_quadprog(::LinModel, weights::ControllerWeights, Ẽ, P̃Δu, P̃u)
+function init_quadprog(
+    ::LinModel, transcription::TranscriptionMethod, weights::ControllerWeights, 
+    Ẽ, P̃Δu, P̃u; warn_cond=1e6
+)
     M_Hp, Ñ_Hc, L_Hp = weights.M_Hp, weights.Ñ_Hc, weights.L_Hp
     H̃ = Hermitian(2*(Ẽ'*M_Hp*Ẽ + P̃Δu'*Ñ_Hc*P̃Δu + P̃u'*L_Hp*P̃u), :L)
+    verify_cond(transcription, H̃, warn_cond)
     return H̃
 end
 "Return empty matrix if `model` is not a [`LinModel`](@ref)."
-function init_quadprog(::SimModel{NT}, weights::ControllerWeights, _, _, _) where {NT<:Real}
+function init_quadprog(
+    ::SimModel{NT}, ::TranscriptionMethod, ::ControllerWeights, args...; kwargs...
+) where {NT<:Real}
     H̃ = Hermitian(zeros(NT, 0, 0), :L)
     return H̃
 end
 
+"Check the condition number of `H̃` and display a `@warn` if `cond(H̃) > warn_cond`."
+function verify_cond(::SingleShooting, H̃, warn_cond)
+    if !isinf(warn_cond)
+        cond_H̃ = cond(H̃)
+        cond_H̃ > warn_cond && @warn(
+            "The Hessian condition number cond_H̃ > $warn_cond. The optimization "*
+            "problem may be ill-conditioned.\n Consider changing the tunings, using "*
+            "MultipleShooting, or using an optimizer more robust to this like DAQP.",
+            cond_H̃,
+        )
+    end
+    return nothing
+end
+"No check if `transcription` is not a `SingleShooting`."
+verify_cond(::TranscriptionMethod,_,_) = nothing
+
 """
     init_defaultcon_mpc(
         estim::StateEstimator, 

src/controller/execute.jl

@@ -609,6 +609,7 @@ end
 "Update the prediction matrices, linear constraints and JuMP optimization."
 function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old)
     model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription
+    weights = mpc.weights
     nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
     optim, con = mpc.optim, mpc.con
     # --- prediction matrices ---
@@ -676,7 +677,8 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old)
     con.x̂0min .-= estim.x̂op # convert x̂ to x̂0 with the new operating point
     con.x̂0max .-= estim.x̂op # convert x̂ to x̂0 with the new operating point
     # --- quadratic programming Hessian matrix ---
-    H̃ = init_quadprog(model, mpc.weights, mpc.Ẽ, mpc.P̃Δu, mpc.P̃u)
+    # do not verify the condition number of the Hessian here:
+    H̃ = init_quadprog(model, transcription, weights, mpc.Ẽ, mpc.P̃Δu, mpc.P̃u; warn_cond=Inf)
     mpc.H̃ .= H̃
     # --- JuMP optimization ---
     Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var]

src/controller/explicitmpc.jl

@@ -57,7 +57,7 @@ struct ExplicitMPC{
         # dummy val (updated just before optimization):
         F = zeros(NT, ny*Hp)
         P̃Δu, P̃u, Ẽ = PΔu, Pu, E # no slack variable ϵ for ExplicitMPC
-        H̃ = init_quadprog(model, weights, Ẽ, P̃Δu, P̃u)
+        H̃ = init_quadprog(model, transcription, weights, Ẽ, P̃Δu, P̃u)
         # dummy vals (updated just before optimization):
         q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
         H̃_chol = cholesky(H̃)
@@ -225,6 +225,7 @@ addinfo!(info, mpc::ExplicitMPC) = info
 "Update the prediction matrices and Cholesky factorization."
 function setmodel_controller!(mpc::ExplicitMPC, uop_old, _ )
     model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription
+    weights = mpc.weights
     nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
     # --- predictions matrices ---
     E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc)
@@ -236,7 +237,8 @@ function setmodel_controller!(mpc::ExplicitMPC, uop_old, _ )
     mpc.V .= V
     mpc.B .= B
     # --- quadratic programming Hessian matrix ---
-    H̃ = init_quadprog(model, mpc.weights, mpc.Ẽ, mpc.P̃Δu, mpc.P̃u)
+    # do not verify the condition number of the Hessian here:
+    H̃ = init_quadprog(model, transcription, weights, mpc.Ẽ, mpc.P̃Δu, mpc.P̃u, warn_cond=Inf)
     mpc.H̃ .= H̃
     set_objective_hessian!(mpc)
     # --- operating points ---

src/controller/linmpc.jl

@@ -79,7 +79,7 @@ struct LinMPC{
             ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
             Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ
         )
-        H̃ = init_quadprog(model, weights, Ẽ, P̃Δu, P̃u)
+        H̃ = init_quadprog(model, transcription, weights, Ẽ, P̃Δu, P̃u)
         # dummy vals (updated just before optimization):
         q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
         Ks, Ps = init_stochpred(estim, Hp)

src/controller/nonlinmpc.jl

@@ -103,7 +103,7 @@ struct NonLinMPC{
             Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ,
             gc!, nc
         )
-        H̃ = init_quadprog(model, weights, Ẽ, P̃Δu, P̃u)
+        H̃ = init_quadprog(model, transcription, weights, Ẽ, P̃Δu, P̃u)
         # dummy vals (updated just before optimization):
         q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
         Ks, Ps = init_stochpred(estim, Hp)