|
| 1 | +# # Stabilization of nonlinear systems |
| 2 | + |
| 3 | +#md # [](@__BINDER_ROOT_URL__/generated/Stabilization of nonlinear systems.ipynb) |
| 4 | +#md # [](@__NBVIEWER_ROOT_URL__/generated/Stabilization of nonlinear systems.ipynb) |
| 5 | +# **Adapted from**: Examples 1, 2 and 3 of [PPA04] |
| 6 | +# |
| 7 | +# [PPA04] Prajna, Stephen, Pablo A. Parrilo, and Anders Rantzer. |
| 8 | +# *Nonlinear control synthesis by convex optimization*. |
| 9 | +# IEEE Transactions on Automatic Control 49.2 (2004): 310-314. |
| 10 | + |
| 11 | +using Test #src |
| 12 | +using DynamicPolynomials |
| 13 | +@polyvar x[1:2] |
| 14 | + |
| 15 | +using SumOfSquares |
| 16 | +using CSDP |
| 17 | +using LinearAlgebra # for ⋅ |
| 18 | +using MultivariatePolynomials |
| 19 | +divergence(f) = sum(differentiate.(f, x)) |
| 20 | +function controller(f, g, b, α, degs) |
| 21 | + solver = optimizer_with_attributes(CSDP.Optimizer, MOI.Silent() => true) |
| 22 | + model = SOSModel(solver) |
| 23 | + a = 1 |
| 24 | + monos = monomials(x, degs) |
| 25 | + N = length(monos) + 1 |
| 26 | + @variable(model, c[1:N] in MOI.NormOneCone(N)) |
| 27 | + c_poly = polynomial(c[2:end], monos) |
| 28 | + fagc = f * a + g * c_poly |
| 29 | + @constraint(model, b * divergence(fagc) - α * differentiate(b, x) ⋅ fagc in SOSCone()) |
| 30 | + @objective(model, Min, c[1]) |
| 31 | + optimize!(model) |
| 32 | + if termination_status(model) != MOI.OPTIMAL |
| 33 | + @warn("Termination status $(termination_status(model)): $(raw_status(model))") |
| 34 | + end |
| 35 | + u = value(c_poly) / value(a) |
| 36 | + return MultivariatePolynomials.mapcoefficientsnz(coef -> abs(coef) < 1e-6 ? 0.0 : coef, u) |
| 37 | +end |
| 38 | + |
| 39 | +import DifferentialEquations, Plots |
| 40 | +function phase_plot(f, quiver_scaling, Δt, X0, solver = DifferentialEquations.Tsit5()) |
| 41 | + ∇(vx, vy) = [fi(x[1] => vx, x[2] => vy) for fi in f] |
| 42 | + ∇pt(v, p, t) = ∇(v[1], v[2]) |
| 43 | + function traj(v0) |
| 44 | + tspan = (0.0, Δt) |
| 45 | + prob = DifferentialEquations.ODEProblem(∇pt, v0, tspan) |
| 46 | + return DifferentialEquations.solve(prob, solver, reltol=1e-8, abstol=1e-8) |
| 47 | + end |
| 48 | + ticks = -5:0.5:5 |
| 49 | + X = repeat(ticks, 1, length(ticks)) |
| 50 | + Y = X' |
| 51 | + Plots.quiver(X, Y, quiver = (x, y) -> ∇(x, y) / quiver_scaling, linewidth=0.5) |
| 52 | + for x0 in X0 |
| 53 | + Plots.plot!(traj(x0), vars=(1, 2), label = nothing) |
| 54 | + end |
| 55 | + Plots.plot!(xlims = (-5, 5), ylims = (-5, 5)) |
| 56 | +end |
| 57 | + |
| 58 | +# ## Example 1 |
| 59 | + |
| 60 | +g = [0, 1] |
| 61 | +f = [x[2] - x[1]^3 + x[1]^2, 0] |
| 62 | +b = 3x[1]^2 + 2x[1]*x[2] + 2x[2]^2 |
| 63 | +u = controller(f, g, b, 4, 0:3) |
| 64 | +@test monomials(u) == [x[2]^3, x[1], x[2]] #src |
| 65 | + |
| 66 | +# We find the controller above which gives the following phase plot. |
| 67 | + |
| 68 | +phase_plot(f + g * u, 200, 10.0, [[x1, x2] for x1 in -5:5:5, x2 in -5:5:5 if x1 != 0 || x2 != 0]) |
| 69 | + |
| 70 | +# ## Example 2 |
| 71 | + |
| 72 | +g = [0, 1] |
| 73 | +f = [2x[1]^3 + x[1]^2*x[2] - 6x[1]*x[2]^2 + 5x[2]^3, 0] |
| 74 | +b = x[1]^2 + x[2]^2 |
| 75 | +u = controller(f, g, b, 2.5, 0:3) |
| 76 | +@test monomials(u) == [x[1]^3, x[1]^2*x[2], x[1]*x[2]^2, x[2]^3] #src |
| 77 | + |
| 78 | +# We find the controller above which gives the following phase plot. |
| 79 | + |
| 80 | +phase_plot(f + g * u, 2000, 5.0, [[-1.0, -5.0], [1.0, 5.0]]) |
| 81 | + |
| 82 | +# ## Example 3 |
| 83 | + |
| 84 | +g = [0, x[2]] |
| 85 | +f = [-6x[1]*x[2]^2 - x[1]^2*x[2] + 2x[2]^3, 0] |
| 86 | +b = x[1]^2 + x[2]^2 |
| 87 | +u = controller(f, g, b, 3, 0:2) |
| 88 | +@test monomials(u) == [x[1]^2, x[2]^2] #src |
| 89 | + |
| 90 | +# We find the controller above which gives the following phase plot. |
| 91 | + |
| 92 | +X0 = [Float64[x1, x2] for x1 in -5:5:5, x2 in -5:5:5 if x2 != 0] |
| 93 | +ε = 1e-4 # We separate the starting point slightly from the hyperplane `x2 = 0` which is invariant. |
| 94 | +push!(X0, [-4, ε]) |
| 95 | +push!(X0, [-3, -ε]) |
| 96 | +push!(X0, [ 3, ε]) |
| 97 | +push!(X0, [ 4, -ε]) |
| 98 | +phase_plot(f + g * u, 2000, 10.0, X0) |
0 commit comments