Merge pull request #2236 from SciML/mc/lin

Consistently initialize before computing the linearization

Author: YingboMa

Project.toml

@@ -31,6 +31,7 @@ LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 MLStyle = "d8e11817-5142-5d16-987a-aa16d5891078"
 MacroTools = "1914dd2f-81c6-5fcd-8719-6d5c9610ff09"
 NaNMath = "77ba4419-2d1f-58cd-9bb1-8ffee604a2e3"
+OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
 RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd"
 Reexport = "189a3867-3050-52da-a836-e630ba90ab69"
 RuntimeGeneratedFunctions = "7e49a35a-f44a-4d26-94aa-eba1b4ca6b47"
@@ -78,6 +79,7 @@ Latexify = "0.11, 0.12, 0.13, 0.14, 0.15, 0.16"
 MLStyle = "0.4.17"
 MacroTools = "0.5"
 NaNMath = "0.3, 1"
+OrdinaryDiffEq = "6"
 RecursiveArrayTools = "2.3"
 Reexport = "0.2, 1"
 RuntimeGeneratedFunctions = "0.5.9"
@@ -107,7 +109,6 @@ NonlinearSolve = "8913a72c-1f9b-4ce2-8d82-65094dcecaec"
 Optimization = "7f7a1694-90dd-40f0-9382-eb1efda571ba"
 OptimizationMOI = "fd9f6733-72f4-499f-8506-86b2bdd0dea1"
 OptimizationOptimJL = "36348300-93cb-4f02-beb5-3c3902f8871e"
-OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
 Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 ReferenceTests = "324d217c-45ce-50fc-942e-d289b448e8cf"
 SafeTestsets = "1bc83da4-3b8d-516f-aca4-4fe02f6d838f"
@@ -120,4 +121,4 @@ Sundials = "c3572dad-4567-51f8-b174-8c6c989267f4"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["AmplNLWriter", "BenchmarkTools", "ControlSystemsMTK", "NonlinearSolve", "ForwardDiff", "Ipopt", "Ipopt_jll", "ModelingToolkitStandardLibrary", "Optimization", "OptimizationOptimJL", "OptimizationMOI", "OrdinaryDiffEq", "Random", "ReferenceTests", "SafeTestsets", "StableRNGs", "Statistics", "SteadyStateDiffEq", "Test", "StochasticDiffEq", "Sundials", "StochasticDelayDiffEq"]
+test = ["AmplNLWriter", "BenchmarkTools", "ControlSystemsMTK", "NonlinearSolve", "ForwardDiff", "Ipopt", "Ipopt_jll", "ModelingToolkitStandardLibrary", "Optimization", "OptimizationOptimJL", "OptimizationMOI", "Random", "ReferenceTests", "SafeTestsets", "StableRNGs", "Statistics", "SteadyStateDiffEq", "Test", "StochasticDiffEq", "Sundials", "StochasticDelayDiffEq"]

src/systems/abstractsystem.jl

@@ -1,3 +1,4 @@
+using OrdinaryDiffEq
 const SYSTEM_COUNT = Threads.Atomic{UInt}(0)
 
 get_component_type(x::AbstractSystem) = get_gui_metadata(x).type
@@ -1249,7 +1250,7 @@ function io_preprocessing(sys::AbstractSystem, inputs,
 end
 
 """
-    lin_fun, simplified_sys = linearization_function(sys::AbstractSystem, inputs, outputs; simplify = false, kwargs...)
+    lin_fun, simplified_sys = linearization_function(sys::AbstractSystem, inputs, outputs; simplify = false, initialize = true, kwargs...)
 
 Return a function that linearizes the system `sys`. The function [`linearize`](@ref) provides a higher-level and easier to use interface.
 
@@ -1273,12 +1274,14 @@ The `simplified_sys` has undergone [`structural_simplify`](@ref) and had any occ
   - `inputs`: A vector of variables that indicate the inputs of the linearized input-output model.
   - `outputs`: A vector of variables that indicate the outputs of the linearized input-output model.
   - `simplify`: Apply simplification in tearing.
+  - `initialize`: If true, a check is performed to ensure that the operating point is consistent (satisfies algebraic equations). If the op is not consistent, initialization is performed.
   - `kwargs`: Are passed on to `find_solvables!`
 
 See also [`linearize`](@ref) which provides a higher-level interface.
 """
 function linearization_function(sys::AbstractSystem, inputs,
     outputs; simplify = false,
+    initialize = true,
     kwargs...)
     sys, diff_idxs, alge_idxs, input_idxs = io_preprocessing(sys, inputs, outputs; simplify,
         kwargs...)
@@ -1294,6 +1297,14 @@ function linearization_function(sys::AbstractSystem, inputs,
             if u !== nothing # Handle systems without states
                 length(sts) == length(u) ||
                     error("Number of state variables ($(length(sts))) does not match the number of input states ($(length(u)))")
+                if initialize && !isempty(alge_idxs) # This is expensive and can be omitted if the user knows that the system is already initialized
+                    residual = fun(u, p, t)
+                    if norm(residual[alge_idxs]) > √(eps(eltype(residual)))
+                        prob = ODEProblem(fun, u, (t, t + 1), p)
+                        integ = init(prob, Rodas5P())
+                        u = integ.u
+                    end
+                end
                 uf = SciMLBase.UJacobianWrapper(fun, t, p)
                 fg_xz = ForwardDiff.jacobian(uf, u)
                 h_xz = ForwardDiff.jacobian(let p = p, t = t
@@ -1397,7 +1408,7 @@ function linearize_symbolic(sys::AbstractSystem, inputs,
             if !allow_input_derivatives
                 der_inds = findall(vec(any(!iszero, Bs, dims = 1)))
                 @show typeof(der_inds)
-                error("Input derivatives appeared in expressions (-g_z\\g_u != 0), the following inputs appeared differentiated: $(ModelingToolkit.inputs(sys)[der_inds]). Call `linear_statespace` with keyword argument `allow_input_derivatives = true` to allow this and have the returned `B` matrix be of double width ($(2nu)), where the last $nu inputs are the derivatives of the first $nu inputs.")
+                error("Input derivatives appeared in expressions (-g_z\\g_u != 0), the following inputs appeared differentiated: $(ModelingToolkit.inputs(sys)[der_inds]). Call `linearize_symbolic` with keyword argument `allow_input_derivatives = true` to allow this and have the returned `B` matrix be of double width ($(2nu)), where the last $nu inputs are the derivatives of the first $nu inputs.")
             end
             B = [B [zeros(nx, nu); Bs]]
             D = [D zeros(ny, nu)]
@@ -1442,8 +1453,8 @@ function markio!(state, orig_inputs, inputs, outputs; check = true)
 end
 
 """
-    (; A, B, C, D), simplified_sys = linearize(sys, inputs, outputs;    t=0.0, op = Dict(), allow_input_derivatives = false, kwargs...)
-    (; A, B, C, D)                 = linearize(simplified_sys, lin_fun; t=0.0, op = Dict(), allow_input_derivatives = false)
+    (; A, B, C, D), simplified_sys = linearize(sys, inputs, outputs;    t=0.0, op = Dict(), allow_input_derivatives = false, zero_dummy_der=false, kwargs...)
+    (; A, B, C, D)                 = linearize(simplified_sys, lin_fun; t=0.0, op = Dict(), allow_input_derivatives = false, zero_dummy_der=false)
 
 Return a NamedTuple with the matrices of a linear statespace representation
 on the form
@@ -1463,6 +1474,8 @@ the default values of `sys` are used.
 
 If `allow_input_derivatives = false`, an error will be thrown if input derivatives (``u̇``) appear as inputs in the linearized equations. If input derivatives are allowed, the returned `B` matrix will be of double width, corresponding to the input `[u; u̇]`.
 
+`zero_dummy_der` can be set to automatically set the operating point to zero for all dummy derivatives.
+
 See also [`linearization_function`](@ref) which provides a lower-level interface, [`linearize_symbolic`](@ref) and [`ModelingToolkit.reorder_states`](@ref).
 
 See extended help for an example.
@@ -1576,7 +1589,7 @@ function linearize(sys, lin_fun; t = 0.0, op = Dict(), allow_input_derivatives =
         if !iszero(Bs)
             if !allow_input_derivatives
                 der_inds = findall(vec(any(!=(0), Bs, dims = 1)))
-                error("Input derivatives appeared in expressions (-g_z\\g_u != 0), the following inputs appeared differentiated: $(inputs(sys)[der_inds]). Call `linear_statespace` with keyword argument `allow_input_derivatives = true` to allow this and have the returned `B` matrix be of double width ($(2nu)), where the last $nu inputs are the derivatives of the first $nu inputs.")
+                error("Input derivatives appeared in expressions (-g_z\\g_u != 0), the following inputs appeared differentiated: $(inputs(sys)[der_inds]). Call `linearize` with keyword argument `allow_input_derivatives = true` to allow this and have the returned `B` matrix be of double width ($(2nu)), where the last $nu inputs are the derivatives of the first $nu inputs.")
             end
             B = [B [zeros(nx, nu); Bs]]
             D = [D zeros(ny, nu)]

src/systems/diffeqs/abstractodesystem.jl

@@ -687,7 +687,6 @@ function get_u0_p(sys,
     use_union = false,
     tofloat = !use_union,
     symbolic_u0 = false)
-    eqs = equations(sys)
     dvs = states(sys)
     ps = parameters(sys)
 

test/linearize.jl

@@ -187,7 +187,7 @@ if VERSION >= v"1.8"
     using ModelingToolkitStandardLibrary
     using ModelingToolkitStandardLibrary.Blocks
     using ModelingToolkitStandardLibrary.Mechanical.MultiBody2D
-    using ModelingToolkitStandardLibrary.Mechanical.Translational
+    using ModelingToolkitStandardLibrary.Mechanical.TranslationalPosition
 
     using ControlSystemsMTK
     using ControlSystemsMTK.ControlSystemsBase: sminreal, minreal, poles
@@ -197,22 +197,20 @@ if VERSION >= v"1.8"
     D = Differential(t)
 
     @named link1 = Link(; m = 0.2, l = 10, I = 1, g = -9.807)
-    @named cart = Translational.Mass(; m = 1, s = 0)
+    @named cart = TranslationalPosition.Mass(; m = 1, s = 0)
     @named fixed = Fixed()
-    @named force = Force()
+    @named force = Force(use_support = false)
 
     eqs = [connect(link1.TX1, cart.flange)
         connect(cart.flange, force.flange)
         connect(link1.TY1, fixed.flange)]
 
     @named model = ODESystem(eqs, t, [], []; systems = [link1, cart, force, fixed])
     def = ModelingToolkit.defaults(model)
-    for s in states(model)
-        def[s] = 0
-    end
-    def[link1.x1] = 10
-    def[link1.fy1] = -def[link1.g] * def[link1.m]
+    def[cart.s] = 10
+    def[cart.v] = 0
     def[link1.A] = -pi / 2
+    def[link1.dA] = 0
     lin_outputs = [cart.s, cart.v, link1.A, link1.dA]
     lin_inputs = [force.f.u]
 
@@ -235,11 +233,12 @@ if VERSION >= v"1.8"
 
     dummyder = setdiff(states(sysss), states(model))
     def = merge(def, Dict(x => 0.0 for x in dummyder))
+    def[link1.fy1] = -def[link1.g] * def[link1.m]
 
     @test substitute(lsyss.A, def) ≈ lsys.A
     # We cannot pivot symbolically, so the part where a linear solve is required
     # is not reliable.
-    @test substitute(lsyss.B, def)[1:6, :] ≈ lsys.B[1:6, :]
+    @test substitute(lsyss.B, def)[1:6, 1] ≈ lsys.B[1:6, 1]
     @test substitute(lsyss.C, def) ≈ lsys.C
     @test substitute(lsyss.D, def) ≈ lsys.D
 end