Use LinearizationOpPoint for trajectory_ss

Project.toml

@@ -1,7 +1,7 @@
 name = "ControlSystemsMTK"
 uuid = "687d7614-c7e5-45fc-bfc3-9ee385575c88"
+version = "2.7.0"
 authors = ["Fredrik Bagge Carlson"]
-version = "2.6.0"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
@@ -17,7 +17,7 @@ UnPack = "3a884ed6-31ef-47d7-9d2a-63182c4928ed"
 [compat]
 ControlSystemsBase = "1.0.1"
 DataInterpolations = "5, 6, 7, 8"
-ModelingToolkit = "11"
+ModelingToolkit = "11.7"
 ModelingToolkitStandardLibrary = "2"
 MonteCarloMeasurements = "1.1"
 RobustAndOptimalControl = "0.4.14"

docs/src/api.md

@@ -28,5 +28,4 @@ ControlSystemsBase.StateSpace
 SymbolicControlSystems.ccode
 SymbolicControlSystems.print_c_array
 ModelingToolkit.reorder_states
-ControlSystemsMTK.fuzz
 ```

docs/src/batch_linearization.md

@@ -123,7 +123,7 @@ for C in Cs
         connect(Ci.output, duffing.u)
     ]
     @named closed_loop = System(eqs, t, systems=[duffing, Ci, fb, ref, F])
-    prob = ODEProblem(structural_simplify(closed_loop), [F.x => 0, F.xd => 0], (0.0, 8.0))
+    prob = ODEProblem(mtkcompile(closed_loop), [F.x => 0, F.xd => 0], (0.0, 8.0))
     sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8)
     plot!(sol, idxs=[duffing.y.u, duffing.u.u], layout=2, lab="")
 end
@@ -137,8 +137,8 @@ eqs = [
     connect(duffing.y, :v, Cgs.scheduling_input) # Don't forget to connect the scheduling variable!
 ]
 @named closed_loop = System(eqs, t, systems=[duffing, Cgs, fb, ref, F])
-prob = ODEProblem(structural_simplify(closed_loop), [F.xd => 0], (0.0, 8.0))
-sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8, initializealg=SciMLBase.NoInit(), dtmax=0.01)
+prob = ODEProblem(mtkcompile(closed_loop), [F.x => 0, F.xd => 0], (0.0, 8.0))
+sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8, dtmax=0.01)
 plot!(sol, idxs=[duffing.y.u, duffing.u.u], l=(2, :red), lab="Gain scheduled")
 plot!(sol, idxs=F.output.u, l=(1, :black, :dash, 0.5), lab="Ref")
 ```
@@ -164,7 +164,7 @@ bodeplot(Ps2, w, legend=false)
 ```
 Not how the closed-loop system changes very little along the trajectory, this is a good indication that the gain-scheduled controller is able to make the system appear linear.
 
-Internally, [`trajectory_ss`](@ref) works very much the same as [`batch_ss`](@ref), but constructs operating points automatically along the trajectory. This requires that the solution contains the states of the simplified system, accessible through the `idxs` argument like `sol(t, idxs=x)`. By linearizing the same system as we simulated, we ensure that this condition holds, doing so requires that we specify the inputs and outputs as analysis points rather than as variables.
+Internally, [`trajectory_ss`](@ref) works very much the same as [`batch_ss`](@ref), but constructs operating points automatically along the trajectory using `ModelingToolkit.LinearizationOpPoint`. The operating points are extracted from the differential states and parameters of the solution. We specify the inputs and outputs as analysis points to properly define the linearization interface.
 
 
 We can replicate the figure above by linearizing the plant and the controller individually, by providing the `loop_openings` argument. When linearizing the plant, we disconnect the controller input by passing `loop_openings=[closed_loop.u]`, and when linearizing the controller, we have various options for disconnecting the the plant:
@@ -221,7 +221,7 @@ plot(
 if we open at both `y` and `v` or we open at `u`, we get controllers for the different values of the scheduling variable, and the corresponding measurement feedback (which is the same as the scheduling variable in this case).
 ```@example BATCHLIN
 using Test
-@test all(sminreal.(controllersv) .== sminreal.(controllersu))
+@test all(isapprox.(sminreal.(controllersv), sminreal.(controllersu), atol=1e-10))
 ```
 
 However, if we only open at `y` we get controller linearizations that _still contain the closed loop through the scheduling connection_ `v`. We can verify this by looking at what variables are present in the input-output map

docs/src/index.md

@@ -192,7 +192,7 @@ symbolic_sys = ss(mats.A, mats.B, mats.C, mats.D)
 That's pretty cool, but even nicer is to generate some code for this symbolic system. Below, we use `build_function` to generate a function that takes a numeric vector `x` representing the values of the state, and a vector of parameters, and returns a `StaticStateSpace{Continuous, Float64}`. We pass the keyword argument `force_SA=true` to `build_function` to get an allocation-free function. 
 
 ```@example LINEAIZE_SYMBOLIC
-defs = ModelingToolkit.defaults(simplified_sys)
+defs = ModelingToolkit.initial_conditions(simplified_sys)
 defs = merge(Dict(unknowns(model) .=> 0), defs)
 x = ModelingToolkit.get_u0(simplified_sys, defs) # Extract the default state and parameter values
 pars = ModelingToolkit.get_p(simplified_sys, defs, split=false)

src/ode_system.jl

@@ -301,7 +301,7 @@ function named_sensitivity_function(
         end
     end
     nu = length(inputs)
-    matrices, ssys = fun(sys, inputs, args...; kwargs...)
+    matrices, ssys, xpt = fun(sys, inputs, args...; kwargs...)
     symstr(x) = Symbol(x isa AnalysisPoint ? x.name : string(x))
     unames = symstr.(inputs)
     fm(x) = convert(Matrix{Float64}, x)
@@ -314,12 +314,16 @@ function named_sensitivity_function(
         lsys = ss(matrices...)
     end
     x_names = get_x_names(lsys, ssys; descriptor, simple_infeigs, balance)
+    u0 = [xpt.p[ModelingToolkit.parameter_index(ssys, i)] for i in ModelingToolkit.inputs(ssys)]
+    xu = (; x=xpt.x, u = u0)
+    extra = Dict(:operating_point => xu)
     nsys = named_ss(
         lsys;
         x = x_names,
         u = unames,
         y = unames, #Symbol.("out_" .* string.(inputs)),
         name = string(Base.nameof(sys)),
+        extra,
     )
     RobustAndOptimalControl.set_extra!(nsys, :ssys, ssys)
     nsys
@@ -488,7 +492,7 @@ centers, radii = fit_complex_perturbations(P * C, w; relative = false, nominal =
 nyquistcircles!(w, centers, radii, ylims = (-4, 1), xlims = (-3, 4))
 ```
 
-See also [`trajectory_ss`](@ref) and [`fuzz`](@ref).
+See also [`trajectory_ss`](@ref).
 """
 function batch_ss(args...; kwargs...)
     lins, ssys, resolved_ops = batch_linearize(args...; kwargs...)
@@ -508,162 +512,46 @@ end
 # end
 
 """
-    linsystems, ssys = trajectory_ss(sys, inputs, outputs, sol; t = _max_100(sol.t), fuzzer=nothing, verbose = true, kwargs...)
+    linsystems, ssys = trajectory_ss(sys, inputs, outputs, sol; t = _max_100(sol.t), kwargs...)
 
 Linearize `sys` around the trajectory `sol` at times `t`. Returns a vector of `StateSpace` objects and the simplified system.
 
+Operating points are extracted from the solution automatically using `ModelingToolkit.LinearizationOpPoint`.
+
 # Arguments:
 - `inputs`: A vector of variables or analysis points.
 - `outputs`: A vector of variables or analysis points.
-- `sol`: An ODE solution object. This solution must contain the states of the simplified system, accessible through the `idxs` argument like `sol(t, idxs=x)`.
+- `sol`: An ODE solution object.
 - `t`: Time points along the solution trajectory at which to linearize. The returned array of `StateSpace` objects will be of the same length as `t`.
-- `fuzzer`: A function that takes an operating point dictionary and returns an array of "fuzzed" operating points. This is useful for adding noise/uncertainty to the operating points along the trajectory. See [`ControlSystemsMTK.fuzz`](@ref) for such a function.
-- `verbose`: If `true`, print warnings for variables that are not found in `sol`.
-- `kwargs`: Are sent to the linearization functions.
+- `kwargs`: Are sent to the linearization functions (e.g., `loop_openings`).
 - `named`: If `true`, the returned systems will be of type `NamedStateSpace`, otherwise they will be of type `StateSpace`.
 """
-function trajectory_ss(sys, inputs, outputs, sol; t = _max_100(sol.t), allow_input_derivatives = false, fuzzer = nothing, verbose = true, named = true, kwargs...)
+function trajectory_ss(sys, inputs, outputs, sol; t = _max_100(sol.t), allow_input_derivatives = false, verbose = true, named = true, kwargs...)
     maximum(t) > maximum(sol.t) && @warn("The maximum time in `t`: $(maximum(t)), is larger than the maximum time in `sol.t`: $(maximum(sol.t)).")
     minimum(t) < minimum(sol.t) && @warn("The minimum time in `t`: $(minimum(t)), is smaller than the minimum time in `sol.t`: $(minimum(sol.t)).")
-    # NOTE: we call linearization_funciton twice :( The first call is to get x=unknowns(ssys), the second call provides the operating points.
-    # lin_fun, ssys = linearization_function(sys, inputs, outputs; warn_initialize_determined = false, kwargs...)
-    lin_fun, ssys = linearization_function(sys, inputs, outputs; warn_empty_op = false, warn_initialize_determined = false, kwargs...)
-    x = unknowns(ssys)
-
-    # TODO: The value of the output (or input) of the input analysis points should be mapped to the perturbation vars
-    perturbation_vars = ModelingToolkit.inputs(ssys)
-    # original_inputs = reduce(vcat, unnamespace(ap) for ap in vcat(inputs)) # assuming all inputs are analysis points for now
-
-    input_names = reduce(vcat, getproperty.(ap.outputs, :u) for ap in vcat(inputs)) 
-    output_names = reduce(vcat, ap.input.u for ap in vcat(outputs)) 
-
-    op_nothing = Dict(unknowns(sys) .=> nothing) # Remove all defaults present in the original system
-    defs = ModelingToolkit.initial_conditions(sys)
-    ops = map(t) do ti
-        opsol = Dict(x => robust_sol_getindex(sol, ti, x, defs; verbose) for x in x)
-        # When the new behavior of Break is introduced, speficy the value for all inupts in ssys by `for x in [x; perturbation_vars]` on the line above
-        # opsolu = Dict(new_u => robust_sol_getindex(sol, ti, u, defs; verbose) for (new_u, u) in zip(perturbation_vars, original_inputs))
-        merge(op_nothing, opsol)
-    end
-    if fuzzer !== nothing
-        opsv = map(ops) do op
-            fuzzer(op)
-        end
-        ops = reduce(vcat, opsv)
-        t = repeat(t, inner = length(ops) ÷ length(t))
-    end
-    lin_fun, ssys = linearization_function(sys, inputs, outputs; op=ops[1], kwargs...)#, initialization_abstol=1e-1, initialization_reltol=1e-1, kwargs...) # initializealg=ModelingToolkit.SciMLBase.NoInit()
-    # Main.lin_fun = lin_fun
-    # Main.op1 = ops[1]
-    # Main.ops = ops 
-    # equations(lin_fun.prob.f.initialization_data.initializeprob.f.sys)
-    # observed(lin_fun.prob.f.initialization_data.initializeprob.f.sys)
-    lins_ops = map(zip(ops, t)) do (op, t)
-        linearize(ssys, lin_fun; op, t, allow_input_derivatives)
-        # linearize(sys, inputs, outputs; op, t, allow_input_derivatives) # useful for debugging
-    end
-    lins = first.(lins_ops)
-    resolved_ops = last.(lins_ops)
+
+    input_names = reduce(vcat, getproperty.(ap.outputs, :u) for ap in vcat(inputs))
+    output_names = reduce(vcat, ap.input.u for ap in vcat(outputs))
+
+    # Use LinearizationOpPoint to let MTK extract operating points from the solution
+    op = ModelingToolkit.LinearizationOpPoint(sol, t)
+    lins, ssys, resolved_ops = linearize(sys, inputs, outputs; op, allow_input_derivatives, kwargs...)
+
     named_linsystems = map(lins) do l
         if named
-            # Convert to a NamedStateSpace with the same names as the original system
             ynames = allunique(output_names) ? symstr.(output_names) : [Symbol(string(nameof(sys))*"_y$i") for i in 1:length(output_names)]
             unames = allunique(input_names) ? symstr.(input_names) : [Symbol(string(nameof(sys))*"_u$i") for i in 1:length(input_names)]
             nsys = named_ss(ss(l.A, l.B, l.C, l.D); name = string(Base.nameof(sys)), x = symstr.(unknowns(ssys)), u = unames, y = ynames)
-            # RobustAndOptimalControl.merge_nonunique_outputs(RobustAndOptimalControl.merge_nonunique_inputs(nsys))
         else
             ss(l.A, l.B, l.C, l.D)
         end
     end
-    (; linsystems = named_linsystems, ssys, ops, resolved_ops)
+    (; linsystems = named_linsystems, ssys, ops = resolved_ops, resolved_ops)
 end
 
 "_max_100(t) = length(t) > 100 ? range(extrema(t)..., 100) : t"
 _max_100(t) = length(t) > 100 ? range(extrema(t)..., 100) : t
 
-"""
-    fuzz(op, p; N = 10, parameters = true, variables = true)
-
-"Fuzz" an operating point `op::Dict` by changing each non-zero value to an uncertain number with multiplicative uncertainty `p`, represented by `N` samples, i.e., `p = 0.1` means that the value is multiplied by a `N` numbers between 0.9 and 1.1.
-
-`parameters` and `variables` indicate whether to fuzz parameters and state variables, respectively.
-
-This function modifies all variables the same way. For more fine-grained control, load the `MonteCarloMeasurements` package and use the `Particles` type directly, followed by `MonteCarloMeasurements.particle_dict2dict_vec(op)`, i.e., the following makes `uncertain_var` uncertain with a 10% uncertainty:
-```julia
-using MonteCarloMeasurements
-op = ModelingToolkit.defaults(sys)
-op[uncertain_var] = op[uncertain_var] * Particles(10, Uniform(0.9, 1.1))
-ops = MonteCarloMeasurements.particle_dict2dict_vec(op)
-batch_ss(model, inputs, outputs, ops)
-```
-If you have more than one uncertain parameter, it's important to use the same number of particles for all of them (10 in the example above).
-
-To make use of this function in [`trajectory_ss`](@ref), pass something like
-```
-fuzzer = op -> ControlSystemsMTK.fuzz(op, 0.02; N=10)
-```
-to fuzz each operating point 10 times with a 2% uncertainty. The resulting number of operating points will increase by 10x.
-"""
-function fuzz(op, p; N=10, parameters = true, variables = true)
-    op = map(collect(keys(op))) do key
-        par = ModelingToolkit.isparameter(key)
-        val = op[key]
-        par && !parameters && return (key => val)
-        !par && !variables && return (key => val)
-        aval = abs(val)
-        uval = issymbolic(val) ? val : iszero(val) ? 0.0 : Particles(N, MonteCarloMeasurements.Uniform(val-p*aval, val+p*aval))
-        key => uval
-    end |> Dict
-    MonteCarloMeasurements.particle_dict2dict_vec(op)
-end
-
-MonteCarloMeasurements.vecindex(p::Symbolics.BasicSymbolic,i) = p
-issymbolic(x) = x isa Union{Symbolics.Num, Symbolics.BasicSymbolic}
-
-"""
-    robust_sol_getindex(sol, ti, x, defs; verbose = true)
-
-Extract symbolic variable `x` from ode solution `sol` at time `ti`. This operation may fail
-- If the variable is a dummy derivative that is not present in the solution. In this case, the value is reconstructed by derivative interpolation.
-- The var is not present at all, in this case, the default value in `defs` is returned.
-
-# Arguments:
-- `sol`: An ODESolution
-- `ti`: Time point
-- `defs`: A Dict with default values. 
-- `verbose`: Print a warning if the variable is not found in the solution.
-"""
-function robust_sol_getindex(sol, ti, x, defs; verbose = true)
-    try
-        return sol(ti, idxs=x)
-    catch
-        n = string((x))
-        if occursin("ˍt(", n)
-            n = split(n, "ˍt(")[1]
-            sp = split(n, '₊')
-            varname = sp[end]
-            local var
-            let t = Symbolics.arguments(Symbolics.unwrap(x))[1]
-                @variables var(t)
-            end
-            ModelingToolkit.@set! var.val.f.name = Symbol(varname)
-            namespaces = sp[1:end-1]
-            if !isempty(namespaces)
-                for ns in reverse(namespaces)
-                    var = ModelingToolkit.renamespace(Symbol(ns), var)
-                end
-            end
-            out = sol(ti, Val{1}, idxs=[Num(var)])[]
-            verbose && println("Could not find variable $x in solution, returning $(out) obtained through interpolation of $var.")
-            return out
-        end
-
-        val = get(defs, x, 0.0)
-        verbose && println("Could not find variable $x in solution, returning $val.")
-        return val
-    end
-end
-
 maybe_interp(interpolator, x, t) = allequal(x) ? x[1] : interpolator(x, t)
 
 """
@@ -712,10 +600,10 @@ function GainScheduledStateSpace(systems, vt; interpolator, x = zeros(systems[1]
         description = "Scheduling variable of gain-scheduled statespace system $name",
     ]
 
-    @variables A(t)[1:nx, 1:nx] = systems[1].A
-    @variables B(t)[1:nx, 1:nu] = systems[1].B
-    @variables C(t)[1:ny, 1:nx] = systems[1].C
-    @variables D(t)[1:ny, 1:nu] = systems[1].D
+    @variables A(t)[1:nx, 1:nx] [guess=systems[1].A]
+    @variables B(t)[1:nx, 1:nu] [guess=systems[1].B]
+    @variables C(t)[1:ny, 1:nx] [guess=systems[1].C]
+    @variables D(t)[1:ny, 1:nu] [guess=systems[1].D]
     A,B,C,D = collect.((A,B,C,D))
 
     eqs = [

test/test_ODESystem.jl

@@ -162,6 +162,8 @@ using ModelingToolkitStandardLibrary.Mechanical.Rotational
 using ModelingToolkitStandardLibrary.Blocks: Sine
 using ModelingToolkit: connect
 import ModelingToolkitStandardLibrary.Blocks
+Spring = Rotational.Spring
+Damper = Rotational.Damper
 t = Blocks.t
 
 # Parameters