From 6ab86dcbebc2dbf4ee7936cbac2c97cdea977513 Mon Sep 17 00:00:00 2001 From: Nicholas Klugman <13633349+nicholaskl97@users.noreply.github.com> Date: Tue, 26 Mar 2024 17:04:32 -0400 Subject: [PATCH 1/3] Added *nonfunctional* demos of planar car and planar quadrotor. TODO: get working before merging with master --- test/planar_car.jl | 285 +++++++++++++++++++++++++++++++++++++++ test/planar_quadrotor.jl | 277 +++++++++++++++++++++++++++++++++++++ test/runtests.jl | 6 + 3 files changed, 568 insertions(+) create mode 100644 test/planar_car.jl create mode 100644 test/planar_quadrotor.jl diff --git a/test/planar_car.jl b/test/planar_car.jl new file mode 100644 index 0000000..2ea394a --- /dev/null +++ b/test/planar_car.jl @@ -0,0 +1,285 @@ +using LinearAlgebra +using NeuralPDE, Lux, ModelingToolkit +using Optimization, OptimizationOptimisers, OptimizationOptimJL, NLopt +using DifferentialEquations +using NeuralLyapunov +using Random +using Test + +Random.seed!(200) + +println("Planar Car - Policy Search") + +######################### Define dynamics and domain ########################## + +function open_loop_car_dynamics(state, u, p, t) + x, y, θ = state + v, ω = u + return [ + v * cos(θ) + v * sin(θ) + ω + ] +end + +lb = [-1.0, -1.0, -π]; +ub = [1.0, 1.0, π]; +goal = [0.0, 0.0, 0.0] +state_syms = [:x, :y, :θ] + +####################### Specify neural Lyapunov problem ####################### + +# Define neural network discretization +# We use an input layer that is periodic with period 2π with respect to θ +dim_state = length(state_syms) +dim_hidden = 15 +dim_phi = 1 +dim_u = 2 +dim_output = dim_phi + dim_u +chain = [Chain( + WrappedFunction(x -> vcat( + x[1:end-1, :], + transpose(sin.(x[end, :])), + transpose(cos.(x[end, :])), + )), + Dense(dim_state + 1, dim_hidden, tanh), + Dense(dim_hidden, dim_hidden, tanh), + Dense(dim_hidden, dim_hidden, tanh), + Dense(dim_hidden, 1) + ) for _ in 1:dim_output] + +# Define neural network discretization +strategy = GridTraining(0.1) +# strategy = QuasiRandomTraining(1000) +discretization = PhysicsInformedNN(chain, strategy) + +# Define neural Lyapunov structure +structure = PositiveSemiDefiniteStructure( + dim_phi; + pos_def = function (state, fixed_point) + θ = state[end] + st = state[1:(end - 1)] + θ_eq = fixed_point[end] + fp = fixed_point[1:(end - 1)] + log(1.0 + (sin(θ) - sin(θ_eq))^2 + (cos(θ) - cos(θ_eq))^2 + (st - fp) ⋅ (st - fp)) + end +) +structure = add_policy_search( + structure, + dim_u +) +minimization_condition = DontCheckNonnegativity() + +# Define Lyapunov decrease condition +# decrease_condition = AsymptoticDecrease(strict = true) +decrease_condition = make_RoA_aware( + LyapunovDecreaseCondition( + true, + (V, dVdt) -> dVdt + 10.0 * V, + function (st, fp) + _st = vcat(st[1:(end - 1)], sin(st[end]), cos(st[end])) + _fp = vcat(fp[1:(end - 1)], sin(fp[end]), cos(fp[end])) + return -1e-5 * (_st - _fp) ⋅ (_st - _fp) + end, + (t) -> max(0.0, t) + ) +) +# Construct neural Lyapunov specification +spec = NeuralLyapunovSpecification( + structure, + minimization_condition, + decrease_condition +) + +############################# Construct PDESystem ############################# + +pde_system, network_func = NeuralLyapunovPDESystem( + open_loop_car_dynamics, + lb, + ub, + spec; + fixed_point = goal, + state_syms = state_syms, + policy_search = true +) + +######################## Construct OptimizationProblem ######################## + +sym_prob = symbolic_discretize(pde_system, discretization); +prob = discretize(pde_system, discretization); + +########################## Solve OptimizationProblem ########################## + +res = Optimization.solve(prob, OptimizationOptimisers.Adam(0.01); maxiters = 300) +prob = Optimization.remake(prob, u0 = res.u) +res = Optimization.solve(prob, OptimizationOptimisers.Adam(0.001); maxiters = 300) +prob = Optimization.remake(prob, u0 = res.u) +res = Optimization.solve(prob, BFGS(); maxiters = 500) + +###################### Get numerical numerical functions ###################### + +V_func, V̇_func = NumericalNeuralLyapunovFunctions( + discretization.phi, + res.u, + network_func, + structure, + open_loop_car_dynamics, + goal +) + +controller = get_policy(discretization.phi, res.u, network_func, dim_u) + +################################## Simulate ################################### + +bounds = [l:0.1:u for (l,u) in zip(lb, ub)] +states = Iterators.map(collect, Iterators.product(bounds...)) +V_predict = vec(V_func(hcat(states...))) +dVdt_predict = vec(V̇_func(hcat(states...))) + +#################################### Tests #################################### + +# Training should result in a fixed point at the upright equilibrium +@test all(isapprox.( + open_loop_car_dynamics(goal, controller(goal), SciMLBase.NullParameters(), 0.0), + 0.0; atol = 1e-8)) + +# V̇ should be negative almost everywhere +@test sum(dVdt_predict .> 0) / length(dVdt_predict) < 1e-3 + +################################## Simulate ################################### + +using Plots + +function plot_planar_car(sol; fixed_point = goal) + x = sol[:x] + y = sol[:y] + θ = sol[:θ] + v = first.(controller.(sol.u)) + vx = v .* cos.(θ) + vy = v .* sin.(θ) + Δt = diff(sol.t) + push!(Δt, Δt[end]) + p = quiver(x, y, quiver = (vx .* Δt, vy .* Δt)) + p = scatter!([x[1]], [y[1]], marker=:o, label="Initial state") + p = scatter!([fixed_point[1]], [fixed_point[2]], marker=:x, label="Goal") + return p +end + + +closed_loop_dynamics = ODEFunction( + (x, p, t) -> open_loop_car_dynamics(x, controller(x), p, t); + sys = SciMLBase.SymbolCache(state_syms) +) + +# Starting at goal +ode_prob = ODEProblem(closed_loop_dynamics, goal, [0.0, 1.0]) +sol = solve(ode_prob, Tsit5()) +# plot(sol) + +# Should make it to the goal +x_end, y_end, θ_end = sol.u[end] +s_end, c_end = sin(θ_end), cos(θ_end) +s_goal, c_goal = sin(goal[end]), cos(goal[end]) +@test all(isapprox.([x_end, y_end, s_end, c_end], vcat(goal[1:end-1], s_goal, c_goal); atol = 1e-5)) + +# Starting in the right direction +x0 = [-0.2, 0.0, 0.0] +@show V_func(x0); +@assert V_func(x0) < 1 +ode_prob = ODEProblem(closed_loop_dynamics, x0, [0.0, 0.3]) +sol = solve(ode_prob, Tsit5()) +plot(sol) +plot_planar_car(sol) + +# Should make it to the goal +x_end, y_end, θ_end = sol.u[end] +s_end, c_end = sin(θ_end), cos(θ_end) +@test all(isapprox.([x_end, y_end, s_end, c_end], vcat(goal[1:end-1], s_goal, c_goal); atol = 1e-3)) + +# Starting in the wrong direction +x0 = [0.0, 0.0, π/4] +@show V_func(x0); +@assert V_func(x0) < 1 +ode_prob = ODEProblem(closed_loop_dynamics, x0, [0.0, 0.3]) +sol = solve(ode_prob, Tsit5()) +plot(sol) +plot_planar_car(sol) + +# Should make it to the goal +x_end, y_end, θ_end = sol.u[end] +s_end, c_end = sin(θ_end), cos(θ_end) +@test all(isapprox.([x_end, y_end, s_end, c_end], vcat(goal[1:end-1], s_goal, c_goal); atol = 0.01)) + + +#= +# Print statistics +println("V(π, 0) = ", V_func(goal)) +println( + "f([π, 0], u([π, 0])) = ", + open_loop_pendulum_dynamics(goal, u(goal), p, 0.0) +) +println( + "V ∋ [", + min(V_func(goal), + minimum(V_predict)), + ", ", + maximum(V_predict), + "]" +) +println( + "V̇ ∋ [", + minimum(dVdt_predict), + ", ", + max(V̇_func(goal), maximum(dVdt_predict)), + "]" +) + +# Plot results +using Plots + +p1 = plot( + xs / pi, + ys, + V_predict, + linetype = + :contourf, + title = "V", + xlabel = "θ/π", + ylabel = "ω", + c = :bone_1 +); +p1 = scatter!([-2 * pi, 0, 2 * pi] / pi, [0, 0, 0], + label = "Downward Equilibria", color = :green, markershape = :+); +p1 = scatter!( + [-pi, pi] / pi, [0, 0], label = "Upward Equilibria", color = :red, markershape = :x); +p2 = plot( + xs / pi, + ys, + dVdt_predict, + linetype = :contourf, + title = "dV/dt", + xlabel = "θ/π", + ylabel = "ω", + c = :binary +); +p2 = scatter!([-2 * pi, 0, 2 * pi] / pi, [0, 0, 0], + label = "Downward Equilibria", color = :green, markershape = :+); +p2 = scatter!([-pi, pi] / pi, [0, 0], label = "Upward Equilibria", + color = :red, markershape = :x, legend = false); +p3 = plot( + xs / pi, + ys, + dVdt_predict .< 0, + linetype = :contourf, + title = "dV/dt < 0", + xlabel = "θ/π", + ylabel = "ω", + colorbar = false, + linewidth = 0 +); +p3 = scatter!([-2 * pi, 0, 2 * pi] / pi, [0, 0, 0], + label = "Downward Equilibria", color = :green, markershape = :+); +p3 = scatter!([-pi, pi] / pi, [0, 0], label = "Upward Equilibria", + color = :red, markershape = :x, legend = false); +plot(p1, p2, p3) +=# diff --git a/test/planar_quadrotor.jl b/test/planar_quadrotor.jl new file mode 100644 index 0000000..edd19e0 --- /dev/null +++ b/test/planar_quadrotor.jl @@ -0,0 +1,277 @@ +using LinearAlgebra +using NeuralPDE, Lux, ModelingToolkit +using Optimization, OptimizationOptimisers, OptimizationOptimJL, NLopt +using NeuralLyapunov +using Random +using Test + +Random.seed!(200) + +println("Planar Quadrotor - Policy Search") + +######################### Define dynamics and domain ########################## + +@parameters m I r g +defaults = Dict([m => 1.0, I => 1.0, r => 1.0, g => 1.0]) + +@variables t x(t) y(t) θ(t) u1(t) [input = true] u2(t) [input = true] +Dt = Differential(t) +DDt = Dt^2 + +eqs = [ + m * DDt(x) ~ -(-u1 + u2) * sin(θ), + m * DDt(y) ~ (u1 + u2) * cos(θ) - m * g, + I * DDt(θ) ~ r * (u1 - u2) +] + +@named planar_quadrotor = ODESystem( + eqs, + t, + [x, y, θ, u1, u2], + [m, I, r, g]; + defaults = defaults +) + +(planar_quadrotor_open_loop_dynamics, _), state_order, p_order = ModelingToolkit.generate_control_function( + planar_quadrotor; simplify = true) + +bounds = [ + θ ∈ (-π, π), + x ∈ (-10.0, 10.0), + y ∈ (-10.0, 10.0), + Dt(θ) ∈ (-10.0, 10.0), + Dt(x) ∈ (-10.0, 10.0), + Dt(y) ∈ (-10.0, 10.0) +] +equilibrium = zeros(length(bounds)); +p = [defaults[param] for param in p_order] + +####################### Specify neural Lyapunov problem ####################### + +# Define neural network discretization +# We use an input layer that is periodic with period 2π with respect to θ +dim_state = length(bounds) +dim_hidden = 20 +dim_phi = 5 +dim_u = 2 +dim_output = dim_phi + dim_u +chain = [Lux.Chain( + Lux.WrappedFunction(x -> vcat( + x[1:(end - 1), :], + transpose(sin.(x[end, :])), + transpose(cos.(x[end, :])) + )), + Dense(7, dim_hidden, tanh), + Dense(dim_hidden, dim_hidden, tanh), + Dense(dim_hidden, dim_hidden, tanh), + Dense(dim_hidden, 1) + ) for _ in 1:dim_output] + +# Define neural network discretization +strategy = QuasiRandomTraining(1000) +discretization = PhysicsInformedNN(chain, strategy) + +# Define neural Lyapunov structure +structure = PositiveSemiDefiniteStructure( + dim_phi; + pos_def = function (state, fixed_point) + θ = state[end] + st = state[1:(end - 1)] + θ_eq = fixed_point[end] + fp = fixed_point[1:(end - 1)] + log(1.0 + (sin(θ) - sin(θ_eq))^2 + (cos(θ) - cos(θ_eq))^2 + (st - fp) ⋅ (st - fp)) + end +) +structure = add_policy_search( + structure, + dim_u +) +minimization_condition = DontCheckNonnegativity() + +# Define Lyapunov decrease condition +decrease_condition = make_RoA_aware( + LyapunovDecreaseCondition( + true, + (V, dVdt) -> dVdt, + function (st, fp) + _st = vcat(st[1:(end - 1)], sin(st[end]), cos(st[end])) + _fp = vcat(fp[1:(end - 1)], sin(fp[end]), cos(fp[end])) + return -1e-5 * (_st - _fp) ⋅ (_st - _fp) + end, + (t) -> max(0.0, t) + ) +) + +# Construct neural Lyapunov specification +spec = NeuralLyapunovSpecification( + structure, + minimization_condition, + decrease_condition +) + +############################# Construct PDESystem ############################# + +pde_system, network_func = NeuralLyapunovPDESystem( + planar_quadrotor, + bounds, + spec +) + +######################## Construct OptimizationProblem ######################## + +sym_prob = symbolic_discretize(pde_system, discretization) +prob = discretize(pde_system, discretization) + +########################## Solve OptimizationProblem ########################## + +res = Optimization.solve(prob, OptimizationOptimisers.Adam(0.01); maxiters = 500) +prob = Optimization.remake(prob, u0 = res.u) +res = Optimization.solve(prob, OptimizationOptimisers.Adam(); maxiters = 500) +prob = Optimization.remake(prob, u0 = res.u) +res = Optimization.solve(prob, BFGS(); maxiters = 300) + +###################### Get numerical numerical functions ###################### + +V_func, V̇_func, ∇V_func = NumericalNeuralLyapunovFunctions( + discretization.phi, + res.u, + network_func, + structure, + planar_quadrotor_open_loop_dynamics, + equilibrium; + p = p +) + +controller = get_policy(discretization.phi, res.u, network_func, dim_u) + +################################## Simulate ################################### + +lb = [-10.0, -10.0, -10.0, -10.0, -10.0, -pi]; +ub = [10.0, 10.0, 10.0, 10.0, 10.0, pi]; + +#= +b = [l:0.02:u for (l,u) in zip(lb, ub)] +states = Iterators.map(collect, Iterators.product(b...)) +V_predict = vec(V_func(hcat(states...))) +dVdt_predict = vec(V̇_func(hcat(states...))) +=# + +#################################### Tests #################################### + +# Training should result in a fixed point at the upright equilibrium +@test all(isapprox.( + planar_quadrotor_open_loop_dynamics(equilibrium, controller(equilibrium), p, 0.0), 0.0; + atol = 1e-8)) + +# V̇ should be negative almost everywhere +#@test sum(dVdt_predict .> 0) / length(dVdt_predict) < 1e-3 + +################################## Simulate ################################### + +using DifferentialEquations + +state_order = map(st -> istree(st) ? operation(st) : st, state_order) +state_syms = Symbol.(state_order) + +closed_loop_dynamics = ODEFunction( + (x, p, t) -> planar_quadrotor_open_loop_dynamics(x, controller(x), p, t); + sys = SciMLBase.SymbolCache(state_syms, Symbol.(p_order)) +) + +# Starting still and too high +x0 = [0.0, 0.0, 0.0, 0.0, 0.1, 0.0] +ode_prob = ODEProblem(closed_loop_dynamics, x0, [0.0, 10.0], p) +sol = solve(ode_prob) +p1 = plot(sol[:x], sol[:y], label = "Trajectory"); +p1 = scatter!([sol[:x][1]], [sol[:y][1]], label = "Start"); +p1 = scatter!([0], [0], label = "Goal"); +p1 = scatter!([sol[:x][end]], [sol[:y][end]], label = "End") + +# Should make it to the top +θ_end, ω_end = sol.u[end] +x_end, y_end = sin(θ_end), -cos(θ_end) +@test all(isapprox.([x_end, y_end, ω_end], [0.0, 1.0, 0.0]; atol = 1e-3)) + +# Starting at a random point +x0 = lb .+ rand(length(bounds)) .* (ub .- lb) +ode_prob = ODEProblem(closed_loop_dynamics, x0, [0.0, 20.0], p) +sol = solve(ode_prob, Tsit5()) +# plot(sol) + +# Should make it to the top +θ_end, ω_end = sol.u[end] +x_end, y_end = sin(θ_end), -cos(θ_end) +@test all(isapprox.([x_end, y_end, ω_end], [0.0, 1.0, 0.0]; atol = 1e-3)) + +#= +# Print statistics +println("V(π, 0) = ", V_func(upright_equilibrium)) +println( + "f([π, 0], u([π, 0])) = ", + controlled_pendulum_dynamics(upright_equilibrium, u(upright_equilibrium), p, 0.0) +) +println( + "V ∋ [", + min(V_func(upright_equilibrium), + minimum(V_predict)), + ", ", + maximum(V_predict), + "]" +) +println( + "V̇ ∋ [", + minimum(dVdt_predict), + ", ", + max(V̇_func(upright_equilibrium), maximum(dVdt_predict)), + "]" +) + +# Plot results +using Plots + +p1 = plot( + xs / pi, + ys, + V_predict, + linetype = + :contourf, + title = "V", + xlabel = "θ/π", + ylabel = "ω", + c = :bone_1 +); +p1 = scatter!([-2 * pi, 0, 2 * pi] / pi, [0, 0, 0], + label = "Downward Equilibria", color = :green, markershape = :+); +p1 = scatter!( + [-pi, pi] / pi, [0, 0], label = "Upward Equilibria", color = :red, markershape = :x); +p2 = plot( + xs / pi, + ys, + dVdt_predict, + linetype = :contourf, + title = "dV/dt", + xlabel = "θ/π", + ylabel = "ω", + c = :binary +); +p2 = scatter!([-2 * pi, 0, 2 * pi] / pi, [0, 0, 0], + label = "Downward Equilibria", color = :green, markershape = :+); +p2 = scatter!([-pi, pi] / pi, [0, 0], label = "Upward Equilibria", + color = :red, markershape = :x, legend = false); +p3 = plot( + xs / pi, + ys, + dVdt_predict .< 0, + linetype = :contourf, + title = "dV/dt < 0", + xlabel = "θ/π", + ylabel = "ω", + colorbar = false, + linewidth = 0 +); +p3 = scatter!([-2 * pi, 0, 2 * pi] / pi, [0, 0, 0], + label = "Downward Equilibria", color = :green, markershape = :+); +p3 = scatter!([-pi, pi] / pi, [0, 0], label = "Upward Equilibria", + color = :red, markershape = :x, legend = false); +plot(p1, p2, p3) +=# diff --git a/test/runtests.jl b/test/runtests.jl index ef1120d..2108d2c 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -16,4 +16,10 @@ using SafeTestsets @time @safetestset "Policy search - inverted pendulum 2" begin include("inverted_pendulum_ODESystem.jl") end + @time @safetestset "Policy search - planar car" begin + include("planar_car.jl") + end + @time @safetestset "Policy search - planar quadrotor" begin + include("planar_quadrotor.jl") + end end From 49c24ed5cdb8ba97acfa3085b05ab6bb2d715195 Mon Sep 17 00:00:00 2001 From: Nicholas Klugman <13633349+nicholaskl97@users.noreply.github.com> Date: Mon, 13 May 2024 15:12:05 -0400 Subject: [PATCH 2/3] Revert "Added *nonfunctional* demos of planar car and planar quadrotor. TODO: get working before merging with master" This reverts commit 6ab86dcbebc2dbf4ee7936cbac2c97cdea977513. --- test/planar_car.jl | 285 --------------------------------------- test/planar_quadrotor.jl | 277 ------------------------------------- test/runtests.jl | 6 - 3 files changed, 568 deletions(-) delete mode 100644 test/planar_car.jl delete mode 100644 test/planar_quadrotor.jl diff --git a/test/planar_car.jl b/test/planar_car.jl deleted file mode 100644 index 2ea394a..0000000 --- a/test/planar_car.jl +++ /dev/null @@ -1,285 +0,0 @@ -using LinearAlgebra -using NeuralPDE, Lux, ModelingToolkit -using Optimization, OptimizationOptimisers, OptimizationOptimJL, NLopt -using DifferentialEquations -using NeuralLyapunov -using Random -using Test - -Random.seed!(200) - -println("Planar Car - Policy Search") - -######################### Define dynamics and domain ########################## - -function open_loop_car_dynamics(state, u, p, t) - x, y, θ = state - v, ω = u - return [ - v * cos(θ) - v * sin(θ) - ω - ] -end - -lb = [-1.0, -1.0, -π]; -ub = [1.0, 1.0, π]; -goal = [0.0, 0.0, 0.0] -state_syms = [:x, :y, :θ] - -####################### Specify neural Lyapunov problem ####################### - -# Define neural network discretization -# We use an input layer that is periodic with period 2π with respect to θ -dim_state = length(state_syms) -dim_hidden = 15 -dim_phi = 1 -dim_u = 2 -dim_output = dim_phi + dim_u -chain = [Chain( - WrappedFunction(x -> vcat( - x[1:end-1, :], - transpose(sin.(x[end, :])), - transpose(cos.(x[end, :])), - )), - Dense(dim_state + 1, dim_hidden, tanh), - Dense(dim_hidden, dim_hidden, tanh), - Dense(dim_hidden, dim_hidden, tanh), - Dense(dim_hidden, 1) - ) for _ in 1:dim_output] - -# Define neural network discretization -strategy = GridTraining(0.1) -# strategy = QuasiRandomTraining(1000) -discretization = PhysicsInformedNN(chain, strategy) - -# Define neural Lyapunov structure -structure = PositiveSemiDefiniteStructure( - dim_phi; - pos_def = function (state, fixed_point) - θ = state[end] - st = state[1:(end - 1)] - θ_eq = fixed_point[end] - fp = fixed_point[1:(end - 1)] - log(1.0 + (sin(θ) - sin(θ_eq))^2 + (cos(θ) - cos(θ_eq))^2 + (st - fp) ⋅ (st - fp)) - end -) -structure = add_policy_search( - structure, - dim_u -) -minimization_condition = DontCheckNonnegativity() - -# Define Lyapunov decrease condition -# decrease_condition = AsymptoticDecrease(strict = true) -decrease_condition = make_RoA_aware( - LyapunovDecreaseCondition( - true, - (V, dVdt) -> dVdt + 10.0 * V, - function (st, fp) - _st = vcat(st[1:(end - 1)], sin(st[end]), cos(st[end])) - _fp = vcat(fp[1:(end - 1)], sin(fp[end]), cos(fp[end])) - return -1e-5 * (_st - _fp) ⋅ (_st - _fp) - end, - (t) -> max(0.0, t) - ) -) -# Construct neural Lyapunov specification -spec = NeuralLyapunovSpecification( - structure, - minimization_condition, - decrease_condition -) - -############################# Construct PDESystem ############################# - -pde_system, network_func = NeuralLyapunovPDESystem( - open_loop_car_dynamics, - lb, - ub, - spec; - fixed_point = goal, - state_syms = state_syms, - policy_search = true -) - -######################## Construct OptimizationProblem ######################## - -sym_prob = symbolic_discretize(pde_system, discretization); -prob = discretize(pde_system, discretization); - -########################## Solve OptimizationProblem ########################## - -res = Optimization.solve(prob, OptimizationOptimisers.Adam(0.01); maxiters = 300) -prob = Optimization.remake(prob, u0 = res.u) -res = Optimization.solve(prob, OptimizationOptimisers.Adam(0.001); maxiters = 300) -prob = Optimization.remake(prob, u0 = res.u) -res = Optimization.solve(prob, BFGS(); maxiters = 500) - -###################### Get numerical numerical functions ###################### - -V_func, V̇_func = NumericalNeuralLyapunovFunctions( - discretization.phi, - res.u, - network_func, - structure, - open_loop_car_dynamics, - goal -) - -controller = get_policy(discretization.phi, res.u, network_func, dim_u) - -################################## Simulate ################################### - -bounds = [l:0.1:u for (l,u) in zip(lb, ub)] -states = Iterators.map(collect, Iterators.product(bounds...)) -V_predict = vec(V_func(hcat(states...))) -dVdt_predict = vec(V̇_func(hcat(states...))) - -#################################### Tests #################################### - -# Training should result in a fixed point at the upright equilibrium -@test all(isapprox.( - open_loop_car_dynamics(goal, controller(goal), SciMLBase.NullParameters(), 0.0), - 0.0; atol = 1e-8)) - -# V̇ should be negative almost everywhere -@test sum(dVdt_predict .> 0) / length(dVdt_predict) < 1e-3 - -################################## Simulate ################################### - -using Plots - -function plot_planar_car(sol; fixed_point = goal) - x = sol[:x] - y = sol[:y] - θ = sol[:θ] - v = first.(controller.(sol.u)) - vx = v .* cos.(θ) - vy = v .* sin.(θ) - Δt = diff(sol.t) - push!(Δt, Δt[end]) - p = quiver(x, y, quiver = (vx .* Δt, vy .* Δt)) - p = scatter!([x[1]], [y[1]], marker=:o, label="Initial state") - p = scatter!([fixed_point[1]], [fixed_point[2]], marker=:x, label="Goal") - return p -end - - -closed_loop_dynamics = ODEFunction( - (x, p, t) -> open_loop_car_dynamics(x, controller(x), p, t); - sys = SciMLBase.SymbolCache(state_syms) -) - -# Starting at goal -ode_prob = ODEProblem(closed_loop_dynamics, goal, [0.0, 1.0]) -sol = solve(ode_prob, Tsit5()) -# plot(sol) - -# Should make it to the goal -x_end, y_end, θ_end = sol.u[end] -s_end, c_end = sin(θ_end), cos(θ_end) -s_goal, c_goal = sin(goal[end]), cos(goal[end]) -@test all(isapprox.([x_end, y_end, s_end, c_end], vcat(goal[1:end-1], s_goal, c_goal); atol = 1e-5)) - -# Starting in the right direction -x0 = [-0.2, 0.0, 0.0] -@show V_func(x0); -@assert V_func(x0) < 1 -ode_prob = ODEProblem(closed_loop_dynamics, x0, [0.0, 0.3]) -sol = solve(ode_prob, Tsit5()) -plot(sol) -plot_planar_car(sol) - -# Should make it to the goal -x_end, y_end, θ_end = sol.u[end] -s_end, c_end = sin(θ_end), cos(θ_end) -@test all(isapprox.([x_end, y_end, s_end, c_end], vcat(goal[1:end-1], s_goal, c_goal); atol = 1e-3)) - -# Starting in the wrong direction -x0 = [0.0, 0.0, π/4] -@show V_func(x0); -@assert V_func(x0) < 1 -ode_prob = ODEProblem(closed_loop_dynamics, x0, [0.0, 0.3]) -sol = solve(ode_prob, Tsit5()) -plot(sol) -plot_planar_car(sol) - -# Should make it to the goal -x_end, y_end, θ_end = sol.u[end] -s_end, c_end = sin(θ_end), cos(θ_end) -@test all(isapprox.([x_end, y_end, s_end, c_end], vcat(goal[1:end-1], s_goal, c_goal); atol = 0.01)) - - -#= -# Print statistics -println("V(π, 0) = ", V_func(goal)) -println( - "f([π, 0], u([π, 0])) = ", - open_loop_pendulum_dynamics(goal, u(goal), p, 0.0) -) -println( - "V ∋ [", - min(V_func(goal), - minimum(V_predict)), - ", ", - maximum(V_predict), - "]" -) -println( - "V̇ ∋ [", - minimum(dVdt_predict), - ", ", - max(V̇_func(goal), maximum(dVdt_predict)), - "]" -) - -# Plot results -using Plots - -p1 = plot( - xs / pi, - ys, - V_predict, - linetype = - :contourf, - title = "V", - xlabel = "θ/π", - ylabel = "ω", - c = :bone_1 -); -p1 = scatter!([-2 * pi, 0, 2 * pi] / pi, [0, 0, 0], - label = "Downward Equilibria", color = :green, markershape = :+); -p1 = scatter!( - [-pi, pi] / pi, [0, 0], label = "Upward Equilibria", color = :red, markershape = :x); -p2 = plot( - xs / pi, - ys, - dVdt_predict, - linetype = :contourf, - title = "dV/dt", - xlabel = "θ/π", - ylabel = "ω", - c = :binary -); -p2 = scatter!([-2 * pi, 0, 2 * pi] / pi, [0, 0, 0], - label = "Downward Equilibria", color = :green, markershape = :+); -p2 = scatter!([-pi, pi] / pi, [0, 0], label = "Upward Equilibria", - color = :red, markershape = :x, legend = false); -p3 = plot( - xs / pi, - ys, - dVdt_predict .< 0, - linetype = :contourf, - title = "dV/dt < 0", - xlabel = "θ/π", - ylabel = "ω", - colorbar = false, - linewidth = 0 -); -p3 = scatter!([-2 * pi, 0, 2 * pi] / pi, [0, 0, 0], - label = "Downward Equilibria", color = :green, markershape = :+); -p3 = scatter!([-pi, pi] / pi, [0, 0], label = "Upward Equilibria", - color = :red, markershape = :x, legend = false); -plot(p1, p2, p3) -=# diff --git a/test/planar_quadrotor.jl b/test/planar_quadrotor.jl deleted file mode 100644 index edd19e0..0000000 --- a/test/planar_quadrotor.jl +++ /dev/null @@ -1,277 +0,0 @@ -using LinearAlgebra -using NeuralPDE, Lux, ModelingToolkit -using Optimization, OptimizationOptimisers, OptimizationOptimJL, NLopt -using NeuralLyapunov -using Random -using Test - -Random.seed!(200) - -println("Planar Quadrotor - Policy Search") - -######################### Define dynamics and domain ########################## - -@parameters m I r g -defaults = Dict([m => 1.0, I => 1.0, r => 1.0, g => 1.0]) - -@variables t x(t) y(t) θ(t) u1(t) [input = true] u2(t) [input = true] -Dt = Differential(t) -DDt = Dt^2 - -eqs = [ - m * DDt(x) ~ -(-u1 + u2) * sin(θ), - m * DDt(y) ~ (u1 + u2) * cos(θ) - m * g, - I * DDt(θ) ~ r * (u1 - u2) -] - -@named planar_quadrotor = ODESystem( - eqs, - t, - [x, y, θ, u1, u2], - [m, I, r, g]; - defaults = defaults -) - -(planar_quadrotor_open_loop_dynamics, _), state_order, p_order = ModelingToolkit.generate_control_function( - planar_quadrotor; simplify = true) - -bounds = [ - θ ∈ (-π, π), - x ∈ (-10.0, 10.0), - y ∈ (-10.0, 10.0), - Dt(θ) ∈ (-10.0, 10.0), - Dt(x) ∈ (-10.0, 10.0), - Dt(y) ∈ (-10.0, 10.0) -] -equilibrium = zeros(length(bounds)); -p = [defaults[param] for param in p_order] - -####################### Specify neural Lyapunov problem ####################### - -# Define neural network discretization -# We use an input layer that is periodic with period 2π with respect to θ -dim_state = length(bounds) -dim_hidden = 20 -dim_phi = 5 -dim_u = 2 -dim_output = dim_phi + dim_u -chain = [Lux.Chain( - Lux.WrappedFunction(x -> vcat( - x[1:(end - 1), :], - transpose(sin.(x[end, :])), - transpose(cos.(x[end, :])) - )), - Dense(7, dim_hidden, tanh), - Dense(dim_hidden, dim_hidden, tanh), - Dense(dim_hidden, dim_hidden, tanh), - Dense(dim_hidden, 1) - ) for _ in 1:dim_output] - -# Define neural network discretization -strategy = QuasiRandomTraining(1000) -discretization = PhysicsInformedNN(chain, strategy) - -# Define neural Lyapunov structure -structure = PositiveSemiDefiniteStructure( - dim_phi; - pos_def = function (state, fixed_point) - θ = state[end] - st = state[1:(end - 1)] - θ_eq = fixed_point[end] - fp = fixed_point[1:(end - 1)] - log(1.0 + (sin(θ) - sin(θ_eq))^2 + (cos(θ) - cos(θ_eq))^2 + (st - fp) ⋅ (st - fp)) - end -) -structure = add_policy_search( - structure, - dim_u -) -minimization_condition = DontCheckNonnegativity() - -# Define Lyapunov decrease condition -decrease_condition = make_RoA_aware( - LyapunovDecreaseCondition( - true, - (V, dVdt) -> dVdt, - function (st, fp) - _st = vcat(st[1:(end - 1)], sin(st[end]), cos(st[end])) - _fp = vcat(fp[1:(end - 1)], sin(fp[end]), cos(fp[end])) - return -1e-5 * (_st - _fp) ⋅ (_st - _fp) - end, - (t) -> max(0.0, t) - ) -) - -# Construct neural Lyapunov specification -spec = NeuralLyapunovSpecification( - structure, - minimization_condition, - decrease_condition -) - -############################# Construct PDESystem ############################# - -pde_system, network_func = NeuralLyapunovPDESystem( - planar_quadrotor, - bounds, - spec -) - -######################## Construct OptimizationProblem ######################## - -sym_prob = symbolic_discretize(pde_system, discretization) -prob = discretize(pde_system, discretization) - -########################## Solve OptimizationProblem ########################## - -res = Optimization.solve(prob, OptimizationOptimisers.Adam(0.01); maxiters = 500) -prob = Optimization.remake(prob, u0 = res.u) -res = Optimization.solve(prob, OptimizationOptimisers.Adam(); maxiters = 500) -prob = Optimization.remake(prob, u0 = res.u) -res = Optimization.solve(prob, BFGS(); maxiters = 300) - -###################### Get numerical numerical functions ###################### - -V_func, V̇_func, ∇V_func = NumericalNeuralLyapunovFunctions( - discretization.phi, - res.u, - network_func, - structure, - planar_quadrotor_open_loop_dynamics, - equilibrium; - p = p -) - -controller = get_policy(discretization.phi, res.u, network_func, dim_u) - -################################## Simulate ################################### - -lb = [-10.0, -10.0, -10.0, -10.0, -10.0, -pi]; -ub = [10.0, 10.0, 10.0, 10.0, 10.0, pi]; - -#= -b = [l:0.02:u for (l,u) in zip(lb, ub)] -states = Iterators.map(collect, Iterators.product(b...)) -V_predict = vec(V_func(hcat(states...))) -dVdt_predict = vec(V̇_func(hcat(states...))) -=# - -#################################### Tests #################################### - -# Training should result in a fixed point at the upright equilibrium -@test all(isapprox.( - planar_quadrotor_open_loop_dynamics(equilibrium, controller(equilibrium), p, 0.0), 0.0; - atol = 1e-8)) - -# V̇ should be negative almost everywhere -#@test sum(dVdt_predict .> 0) / length(dVdt_predict) < 1e-3 - -################################## Simulate ################################### - -using DifferentialEquations - -state_order = map(st -> istree(st) ? operation(st) : st, state_order) -state_syms = Symbol.(state_order) - -closed_loop_dynamics = ODEFunction( - (x, p, t) -> planar_quadrotor_open_loop_dynamics(x, controller(x), p, t); - sys = SciMLBase.SymbolCache(state_syms, Symbol.(p_order)) -) - -# Starting still and too high -x0 = [0.0, 0.0, 0.0, 0.0, 0.1, 0.0] -ode_prob = ODEProblem(closed_loop_dynamics, x0, [0.0, 10.0], p) -sol = solve(ode_prob) -p1 = plot(sol[:x], sol[:y], label = "Trajectory"); -p1 = scatter!([sol[:x][1]], [sol[:y][1]], label = "Start"); -p1 = scatter!([0], [0], label = "Goal"); -p1 = scatter!([sol[:x][end]], [sol[:y][end]], label = "End") - -# Should make it to the top -θ_end, ω_end = sol.u[end] -x_end, y_end = sin(θ_end), -cos(θ_end) -@test all(isapprox.([x_end, y_end, ω_end], [0.0, 1.0, 0.0]; atol = 1e-3)) - -# Starting at a random point -x0 = lb .+ rand(length(bounds)) .* (ub .- lb) -ode_prob = ODEProblem(closed_loop_dynamics, x0, [0.0, 20.0], p) -sol = solve(ode_prob, Tsit5()) -# plot(sol) - -# Should make it to the top -θ_end, ω_end = sol.u[end] -x_end, y_end = sin(θ_end), -cos(θ_end) -@test all(isapprox.([x_end, y_end, ω_end], [0.0, 1.0, 0.0]; atol = 1e-3)) - -#= -# Print statistics -println("V(π, 0) = ", V_func(upright_equilibrium)) -println( - "f([π, 0], u([π, 0])) = ", - controlled_pendulum_dynamics(upright_equilibrium, u(upright_equilibrium), p, 0.0) -) -println( - "V ∋ [", - min(V_func(upright_equilibrium), - minimum(V_predict)), - ", ", - maximum(V_predict), - "]" -) -println( - "V̇ ∋ [", - minimum(dVdt_predict), - ", ", - max(V̇_func(upright_equilibrium), maximum(dVdt_predict)), - "]" -) - -# Plot results -using Plots - -p1 = plot( - xs / pi, - ys, - V_predict, - linetype = - :contourf, - title = "V", - xlabel = "θ/π", - ylabel = "ω", - c = :bone_1 -); -p1 = scatter!([-2 * pi, 0, 2 * pi] / pi, [0, 0, 0], - label = "Downward Equilibria", color = :green, markershape = :+); -p1 = scatter!( - [-pi, pi] / pi, [0, 0], label = "Upward Equilibria", color = :red, markershape = :x); -p2 = plot( - xs / pi, - ys, - dVdt_predict, - linetype = :contourf, - title = "dV/dt", - xlabel = "θ/π", - ylabel = "ω", - c = :binary -); -p2 = scatter!([-2 * pi, 0, 2 * pi] / pi, [0, 0, 0], - label = "Downward Equilibria", color = :green, markershape = :+); -p2 = scatter!([-pi, pi] / pi, [0, 0], label = "Upward Equilibria", - color = :red, markershape = :x, legend = false); -p3 = plot( - xs / pi, - ys, - dVdt_predict .< 0, - linetype = :contourf, - title = "dV/dt < 0", - xlabel = "θ/π", - ylabel = "ω", - colorbar = false, - linewidth = 0 -); -p3 = scatter!([-2 * pi, 0, 2 * pi] / pi, [0, 0, 0], - label = "Downward Equilibria", color = :green, markershape = :+); -p3 = scatter!([-pi, pi] / pi, [0, 0], label = "Upward Equilibria", - color = :red, markershape = :x, legend = false); -plot(p1, p2, p3) -=# diff --git a/test/runtests.jl b/test/runtests.jl index 2108d2c..ef1120d 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -16,10 +16,4 @@ using SafeTestsets @time @safetestset "Policy search - inverted pendulum 2" begin include("inverted_pendulum_ODESystem.jl") end - @time @safetestset "Policy search - planar car" begin - include("planar_car.jl") - end - @time @safetestset "Policy search - planar quadrotor" begin - include("planar_quadrotor.jl") - end end From 1ee202f9a05c7b1bd586d23bc142d5a36f843f71 Mon Sep 17 00:00:00 2001 From: Nicholas Klugman <13633349+nicholaskl97@users.noreply.github.com> Date: Tue, 14 May 2024 12:24:27 -0400 Subject: [PATCH 3/3] Started using `PeriodicEmbedding` in demos instead of `WrappedFunction` --- Project.toml | 2 +- test/damped_pendulum.jl | 6 +----- test/inverted_pendulum.jl | 10 +++------- test/inverted_pendulum_ODESystem.jl | 6 +----- 4 files changed, 6 insertions(+), 18 deletions(-) diff --git a/Project.toml b/Project.toml index 2ac2161..57cdf6e 100644 --- a/Project.toml +++ b/Project.toml @@ -13,7 +13,7 @@ SciMLBase = "0bca4576-84f4-4d90-8ffe-ffa030f20462" [compat] ForwardDiff = "0.10" JuMP = "1" -Lux = "0.5" +Lux = "0.5.45" ModelingToolkit = "8, 9" NLopt = "1" NeuralPDE = "5.10" diff --git a/test/damped_pendulum.jl b/test/damped_pendulum.jl index b5ef2dc..df60974 100644 --- a/test/damped_pendulum.jl +++ b/test/damped_pendulum.jl @@ -45,11 +45,7 @@ dim_state = length(bounds) dim_hidden = 15 dim_output = 2 chain = [Lux.Chain( - Lux.WrappedFunction(x -> vcat( - transpose(sin.(x[1, :])), - transpose(cos.(x[1, :])), - transpose(x[2, :]) - )), + PeriodicEmbedding([1], [2π]), Dense(3, dim_hidden, tanh), Dense(dim_hidden, dim_hidden, tanh), Dense(dim_hidden, 1, use_bias = false) diff --git a/test/inverted_pendulum.jl b/test/inverted_pendulum.jl index 6433995..ef9e274 100644 --- a/test/inverted_pendulum.jl +++ b/test/inverted_pendulum.jl @@ -36,11 +36,7 @@ dim_phi = 2 dim_u = 1 dim_output = dim_phi + dim_u chain = [Lux.Chain( - Lux.WrappedFunction(x -> vcat( - transpose(sin.(x[1, :])), - transpose(cos.(x[1, :])), - transpose(x[2, :]) - )), + PeriodicEmbedding([1], [2π]), Dense(3, dim_hidden, tanh), Dense(dim_hidden, dim_hidden, tanh), Dense(dim_hidden, 1, use_bias = false) @@ -151,7 +147,7 @@ closed_loop_dynamics = ODEFunction( # Starting still at bottom downward_equilibrium = zeros(2) -ode_prob = ODEProblem(closed_loop_dynamics, downward_equilibrium, [0.0, 35.0], p) +ode_prob = ODEProblem(closed_loop_dynamics, downward_equilibrium, [0.0, 75.0], p) sol = solve(ode_prob, Tsit5()) # plot(sol) @@ -162,7 +158,7 @@ x_end, y_end = sin(θ_end), -cos(θ_end) # Starting at a random point x0 = lb .+ rand(2) .* (ub .- lb) -ode_prob = ODEProblem(closed_loop_dynamics, x0, [0.0, 20.0], p) +ode_prob = ODEProblem(closed_loop_dynamics, x0, [0.0, 75.0], p) sol = solve(ode_prob, Tsit5()) # plot(sol) diff --git a/test/inverted_pendulum_ODESystem.jl b/test/inverted_pendulum_ODESystem.jl index caff5e4..03f02bb 100644 --- a/test/inverted_pendulum_ODESystem.jl +++ b/test/inverted_pendulum_ODESystem.jl @@ -49,11 +49,7 @@ dim_phi = 2 dim_u = 1 dim_output = dim_phi + dim_u chain = [Lux.Chain( - Lux.WrappedFunction(x -> vcat( - transpose(sin.(x[1, :])), - transpose(cos.(x[1, :])), - transpose(x[2, :]) - )), + PeriodicEmbedding([1], [2π]), Dense(3, dim_hidden, tanh), Dense(dim_hidden, dim_hidden, tanh), Dense(dim_hidden, 1, use_bias = false)