diff --git a/src/ControlSystemsMTK.jl b/src/ControlSystemsMTK.jl index 1e987e9..e7e1b62 100644 --- a/src/ControlSystemsMTK.jl +++ b/src/ControlSystemsMTK.jl @@ -35,6 +35,23 @@ export build_quadratic_cost_matrix export get_named_sensitivity, get_named_comp_sensitivity, get_named_looptransfer +""" +Default keyword arguments forwarded to ModelingToolkit's linearization (and through it to +`mtkcompile`) by every linearization-based function in this package (`named_ss`, +`get_named_sensitivity` and friends, `batch_ss`, `trajectory_ss`, +`build_quadratic_cost_matrix`). `inline_linear_sccs` makes tearing solve linear +strongly-connected components inline (symbolically for size ≤ `analytical_linear_scc_limit`) +rather than through numerical linear-solve calls embedded in the generated code. This is +required for correct linearization of models with large linear SCCs (e.g. multibody +mechanisms). Keyword arguments passed by the caller take precedence, so pass +`reassemble_alg = ModelingToolkit.StructuralTransformations.DefaultReassembleAlgorithm()` to +restore the ModelingToolkit default. +""" +const DEFAULT_LINEARIZE_KWARGS = (; + reassemble_alg = ModelingToolkit.StructuralTransformations.DefaultReassembleAlgorithm(; + inline_linear_sccs = true, analytical_linear_scc_limit = 1), +) + include("ode_system.jl") # include("symbolic_optimization.jl") diff --git a/src/ode_system.jl b/src/ode_system.jl index a8fad8f..7eee5b5 100644 --- a/src/ode_system.jl +++ b/src/ode_system.jl @@ -110,12 +110,16 @@ function RobustAndOptimalControl.named_ss( out end end - matrices, ssys, xpt = ModelingToolkit.linearize(sys, inputs, outputs; kwargs...) + matrices, ssys, xpt = ModelingToolkit.linearize(sys, inputs, outputs; DEFAULT_LINEARIZE_KWARGS..., kwargs...) unames = symstr.(inputs) if nu > 0 && size(matrices.B, 2) == 2nu # This indicates that input derivatives are present - duinds = findall(any(!iszero, eachcol(matrices.B[:, nu+1:end]))) .+ nu - u2du = (1:nu) .=> duinds # This maps inputs to their derivatives + # Derivative column nu + i is the derivative of input i (see the MTK docs for + # `linearize` with `allow_input_derivatives = true`), so the pairing is positional. + # All-zero derivative columns contribute nothing and are included for simplicity; + # a pairing derived from the set of nonzero columns mispairs inputs when only a + # subset of the derivative columns is nonzero. + u2du = [i => i + nu for i in 1:nu] # This maps inputs to their derivatives lsys = causal_simplification(matrices, u2du; descriptor, simple_infeigs, big, balance) else lsys = ss(matrices...) @@ -154,8 +158,12 @@ function RobustAndOptimalControl.named_ss( unames = symstr.(inputs) if nu > 0 && size(matrices.B, 2) == 2nu # This indicates that input derivatives are present - duinds = findall(any(!iszero, eachcol(matrices.B[:, nu+1:end]))) .+ nu - u2du = (1:nu) .=> duinds # This maps inputs to their derivatives + # Derivative column nu + i is the derivative of input i (see the MTK docs for + # `linearize` with `allow_input_derivatives = true`), so the pairing is positional. + # All-zero derivative columns contribute nothing and are included for simplicity; + # a pairing derived from the set of nonzero columns mispairs inputs when only a + # subset of the derivative columns is nonzero. + u2du = [i => i + nu for i in 1:nu] # This maps inputs to their derivatives lsys = causal_simplification(matrices, u2du; descriptor, simple_infeigs, big, balance, verbose=false) else lsys = ss(matrices...) @@ -301,14 +309,18 @@ function named_sensitivity_function( end end nu = length(inputs) - matrices, ssys = fun(sys, inputs, args...; kwargs...) + matrices, ssys = fun(sys, inputs, args...; DEFAULT_LINEARIZE_KWARGS..., kwargs...) symstr(x) = Symbol(x isa AnalysisPoint ? x.name : string(x)) unames = symstr.(inputs) fm(x) = convert(Matrix{Float64}, x) if nu > 0 && size(matrices.B, 2) == 2nu # This indicates that input derivatives are present - duinds = findall(any(!iszero, eachcol(matrices.B[:, nu+1:end]))) .+ nu - u2du = (1:nu) .=> duinds # This maps inputs to their derivatives + # Derivative column nu + i is the derivative of input i (see the MTK docs for + # `linearize` with `allow_input_derivatives = true`), so the pairing is positional. + # All-zero derivative columns contribute nothing and are included for simplicity; + # a pairing derived from the set of nonzero columns mispairs inputs when only a + # subset of the derivative columns is nonzero. + u2du = [i => i + nu for i in 1:nu] # This maps inputs to their derivatives lsys = causal_simplification(matrices, u2du; descriptor, simple_infeigs, big, balance) else lsys = ss(matrices...) @@ -384,7 +396,7 @@ The second problem above, the ordering of the states, can be worked around using - `costs`: A vector of pairs. """ function build_quadratic_cost_matrix(sys::System, inputs::AbstractVector, costs::AbstractVector{<:Pair}; kwargs...) - matrices, ssys, extras = ModelingToolkit.linearize(sys, inputs, first.(costs); kwargs...) + matrices, ssys, extras = ModelingToolkit.linearize(sys, inputs, first.(costs); DEFAULT_LINEARIZE_KWARGS..., kwargs...) x = ModelingToolkit.unknowns(ssys) y = ModelingToolkit.outputs(ssys) nx = length(x) @@ -406,7 +418,7 @@ end function batch_linearize(sys, inputs, outputs, ops::AbstractVector{<:AbstractDict}; t = 0.0, allow_input_derivatives = false, kwargs...) - lin_fun, ssys = linearization_function(sys, inputs, outputs; op=ops[1], kwargs...) + lin_fun, ssys = linearization_function(sys, inputs, outputs; op=ops[1], DEFAULT_LINEARIZE_KWARGS..., kwargs...) lins_ops = map(ops) do op linearize(ssys, lin_fun; op, t, allow_input_derivatives) end @@ -552,7 +564,7 @@ function trajectory_ss(sys, inputs, outputs, sol; t = _max_100(sol.t), allow_inp 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() + lin_fun, ssys = linearization_function(sys, inputs, outputs; op=ops[1], DEFAULT_LINEARIZE_KWARGS..., 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