LQR controlled pendulum ROA using SOS

1_roa.jl

using MatrixEquations
using DifferentialEquations
using ForwardDiff
using Plots
using LinearAlgebra
using DynamicPolynomials
using SumOfSquares

using MosekTools, Mosek

# model
g = 9.81
m = 1
l = 1
b = 0.1

# directional vector functions
## additional rotation of the directional vector by θ
increment(u, θ) = [cos(θ) sin(θ); -sin(θ) cos(θ)] * u[1:2]

## angle between two directional vectors (one is a polynomial approximation corresponding to the cross product)
difference(u2, u1, poly=false) = poly ? u1[2] * u2[1] - u1[1] * u2[2] : atan(u2[1], u2[2]) - atan(u1[1], u1[2])

# System's dynamics in a polynomial form (x[1] == sin(θ), x[2] == cos(θ))
open_loop_dynamics(x, u) = [x[2] * x[3], -x[1] * x[3], (-g * m * l * x[1] - b * x[3] + u[1]) / (m * l^2)]

# LQR design
x_eq = [0, -1, 0]
u_eq = [0]

Q = diagm([10, 1])
R = diagm([1e-2])

state_increment(x, dx) = vcat(increment(x[1:2], dx[1]), x[3] + dx[2])
state_difference(x2, x1, poly=false) = vcat(difference(x2[1:2], x1[1:2], poly), x2[3] - x1[3])

A = ForwardDiff.jacobian(dx_ -> state_difference(open_loop_dynamics(state_increment(x_eq, dx_), u_eq), x_eq, true), zeros(2))
B = ForwardDiff.jacobian(u_ -> state_difference(open_loop_dynamics(x_eq, u_), x_eq, true), u_eq)

S, _ = arec(A, B, R, Q)
K = inv(R) * B' * S

# Closed-loop dynamics with the LQR
closed_loop_dynamics(x) = open_loop_dynamics(x, u_eq - K * state_difference(x, x_eq, true))

# Simulation (shows the system is stable)
tspan = (0.0, 1)
x0 = [sin(0.9 * pi), cos(0.9 * pi), 0]

prob = ODEProblem((x_, _, _) -> closed_loop_dynamics(x_), x0, tspan)
sol = solve(prob)

ts = tspan[1]:1e-3:tspan[2]
plt = plot(ts, mapreduce(t -> state_difference(sol(t), [0, 1, 0])', vcat, ts))
display(plt)

# SOS optimization
## states
@polyvar s c ω
x = [s, c, ω]

## dynamics and Lyapunov function
f = closed_loop_dynamics(x)

dx = state_difference(x, x0, true)
V = dot(dx, S * dx)
dVdt = dot(differentiate(V, x), f)

## Model
model = SOSModel(Mosek.Optimizer)

@variable(model, ρ)
@objective(model, Max, ρ)

n = monomials(x, 0:3)
@variable(model, λ, Poly(n))
@variable(model, μ, Poly(n))

pos = dot(x - x_eq, x - x_eq)
@constraint(model, pos * (V - ρ) + μ * (s^2 + c^2 - 1) + λ * dVdt >= 0)

optimize!(model)
value(ρ)

2_ad-hoc-controller.jl

using MatrixEquations
using DifferentialEquations
using ForwardDiff
using Plots
using LinearAlgebra
using DynamicPolynomials
using SumOfSquares

using MosekTools, Mosek

# model
g = 9.81
m = 1
l = 1
b = 0.1

# System's dynamics in a polynomial form (x[1] == sin(θ), x[2] == cos(θ))
open_loop_dynamics(x, u) = [
    x[2] * x[3],
    -x[1] * x[3],
    (-g * m * l * x[1] - b * x[3] + u[1]) / (m * l^2)
]

# Utility for generating a state of the polynomial system
state_generator(θ, ω) = [sin(θ), cos(θ), ω]

# Controller and associated Lyapunov function
controller(x) = [10 * x[1] - 0.5 * x[1]^3 - 0.5 * x[3]]
lyapunov_function(x) = g * m * l * (1 + x[2]) + m * l^2 * x[3]^2

# Closed-loop dynamics with the LQR
closed_loop_dynamics(x) = open_loop_dynamics(x, controller(x))

# Simulation (shows the system is stable)
tspan = (0.0, 30)
x0 = state_generator(3 / 4 * pi, 0) # unstabilizable
x0 = state_generator(4 / 5 * pi, 0) # stabilizable

prob = ODEProblem((x_, _, _) -> closed_loop_dynamics(x_), x0, tspan)
sol = solve(prob)

ts = tspan[1]:1e-3:tspan[2]
xs = map(t -> sol(t), ts)
plt = plot(ts, mapreduce(x -> x', vcat, xs), label=["sin" "cos" "omega"])
plot!(plt, ts, map(x -> lyapunov_function(x), xs), label="V")
display(plt)

# SOS optimization
## states
@polyvar s c ω
x = [s, c, ω]

## dynamics and Lyapunov function
f = closed_loop_dynamics(x)
V = lyapunov_function(x)
dVdt = dot(differentiate(V, x), f)

## Model
model = SOSModel(Mosek.Optimizer)

@variable(model, ρ)
@objective(model, Max, ρ)

n = monomials(x, 0:3)
@variable(model, λ, Poly(n))
@variable(model, μ, Poly(n))

x_eq = [0, -1, 0]
pos = dot(x - x_eq, x - x_eq)
@constraint(model, pos * (V - ρ) + μ * (s^2 + c^2 - 1) + λ * dVdt >= 0)

set_start_value(ρ, 0.1)
optimize!(model)
display(termination_status(model))
value(ρ)

3_alternating_optimizations.jl

using MatrixEquations
using DifferentialEquations
using ForwardDiff
using Plots
using LinearAlgebra
using DynamicPolynomials
using SumOfSquares

using MosekTools, Mosek

# model
g = 9.81
m = 1
l = 1
b = 0.1

# System's dynamics in a polynomial form (x[1] == sin(θ), x[2] == cos(θ))
open_loop_dynamics(x, u) = [
    x[2] * x[3],
    -x[1] * x[3],
    (-g * m * l * x[1] - b * x[3] + u[1]) / (m * l^2)
]

# Utility for generating a state of the polynomial system
state_generator(θ, ω) = [sin(θ), cos(θ), ω]

# Controller and associated Lyapunov function
controller(x) = [10 * x[1] - 0.5 * x[1]^3 - 0.5 * x[3]]
lyapunov_function(x) =  g * m * l * (1 + x[2]) + m * l^2 * x[3]^2

# Closed-loop dynamics with the LQR
closed_loop_dynamics(x) = open_loop_dynamics(x, controller(x))

# Simulation (shows the system is stable)
tspan = (0.0, 30)
x0 = state_generator(3 / 4 * pi, 0) # unstabilizable
x0 = state_generator(4 / 5 * pi, 0) # stabilizable

prob = ODEProblem((x_, _, _) -> closed_loop_dynamics(x_), x0, tspan)
sol = solve(prob)

ts = tspan[1]:1e-3:tspan[2]
xs = map(t -> sol(t), ts)
plt = plot(ts, mapreduce(x -> x', vcat, xs), label=["sin" "cos" "omega"])
plot!(plt, ts, map(x -> lyapunov_function(x), xs), label="V")
display(plt)

# SOS optimization
## states
@polyvar s c ω
x = [s, c, ω]

## dynamics and Lyapunov function
f = closed_loop_dynamics(x)
V = lyapunov_function(x)
dVdt = dot(differentiate(V, x), f)

## monomial basis and set of interest
n = monomials(x, 0:6)

S = @set s^2 + c^2 == 1

## Model
function optimize_λ(ρ)
    model = SOSModel(Mosek.Optimizer)

    @variable(model, λ, Poly(n))
    @constraint(model, λ >= 0)
    @constraint(model, λ * (V - ρ) - dVdt >= 0, domain = S)

    optimize!(model)
    display(termination_status(model))
    return value(λ.a)
end

function optimize_ρ(a)
    λ = a' * n

    model = SOSModel(Mosek.Optimizer)

    @variable(model, ρ)
    @objective(model, Max, ρ)
    @constraint(model, λ * (V - ρ) - dVdt >= 0, domain = S)

    optimize!(model)
    display(termination_status(model))
    return value(ρ)
end

ρ = 0.1
a = Vector{Float64}(undef, length(n))

for _ in 1:1
    a .= optimize_λ(ρ)
    global ρ = optimize_ρ(a)
    display(ρ)
end