Skip to content

Commit

Permalink
Adds StableRNGs for NeuralLyapunovProblemLibrary tests
Browse files Browse the repository at this point in the history
  • Loading branch information
nicholaskl97 committed Feb 28, 2025
1 parent c30dbc8 commit d744e09
Show file tree
Hide file tree
Showing 6 changed files with 61 additions and 33 deletions.
4 changes: 3 additions & 1 deletion lib/NeuralLyapunovProblemLibrary/Project.toml
Original file line number Diff line number Diff line change
Expand Up @@ -22,14 +22,16 @@ OrdinaryDiffEq = "6.91.0"
Plots = "1.40.9"
Rotations = "1.7.1"
SafeTestsets = "0.1.0"
StableRNGs = "1.0.2"
julia = "1.11"

[extras]
ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
SafeTestsets = "1bc83da4-3b8d-516f-aca4-4fe02f6d838f"
StableRNGs = "860ef19b-820b-49d6-a774-d7a799459cd3"
Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"

[targets]
test = ["Test", "ControlSystemsBase", "OrdinaryDiffEq", "Plots", "SafeTestsets"]
test = ["Test", "ControlSystemsBase", "OrdinaryDiffEq", "Plots", "SafeTestsets", "StableRNGs"]
4 changes: 2 additions & 2 deletions lib/NeuralLyapunovProblemLibrary/src/quadrotor.jl
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@ Dt = Differential(t); DDt = Dt^2
@variables u1(t) [input=true] u2(t) [input=true]
@parameters m I_quad g r

# Thrusts must be positive
# Thrusts must be nonnegative
ũ1 = max(0, u1)
ũ2 = max(0, u2)

Expand Down Expand Up @@ -46,7 +46,7 @@ angular_velocity_world = [ωφ, ωθ, ωψ]
@variables T(t) [input=true]
@variables τφ(t) [input=true] τθ(t) [input=true] τψ(t) [input=true]

# Individual rotor thrusts must be positive
# Individual rotor thrusts must be nonnegative
f = ([1 0 -2 -1; 1 2 0 1; 1 0 2 -1; 1 -2 0 1] * [T; τφ; τθ; τψ]) ./ 4
= max.(0, f)
= [1 1 1 1; 0 1 0 -1; -1 0 1 0; -1 1 -1 1] *
Expand Down
28 changes: 18 additions & 10 deletions lib/NeuralLyapunovProblemLibrary/test/double_pendulum_test.jl
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,9 @@ using OrdinaryDiffEq
using Plots
using LinearAlgebra
using ControlSystemsBase: lqr, Continuous
using Test
using Test, StableRNGs

rng = StableRNG(0)

################################## Double pendulum energy ##################################
function U(x, p)
Expand All @@ -25,10 +27,12 @@ end
E(x, p) = T(x, p) + U(x, p)

######################### Undriven double pendulum conserve energy #########################
t = independent_variable(double_pendulum_undriven)
println("Undriven double pendulum energy conservation test")

t, = independent_variables(double_pendulum_undriven)
Dt = Differential(t)
θ1, θ2 = unknowns(double_pendulum_undriven)
x0 = Dict([θ1, θ2, Dt(θ1), Dt(θ2)] .=> vcat(2π * rand(2) .- π, zeros(2)))
x0 = Dict([θ1, θ2, Dt(θ1), Dt(θ2)] .=> vcat(2π * rand(rng, 2) .- π, zeros(2)))

# Assume uniform rods of random mass and length
m1, m2 = ones(2)
Expand Down Expand Up @@ -68,6 +72,8 @@ anim = plot_double_pendulum(sol, p)
# gif(anim, fps=50)

########################### Feedback cancellation, PD controller ###########################
println("Double pendulum feedback cancellation test")

function π_cancellation(x, p)
θ1, θ2, ω1, ω2 = x
I1, I2, l1, l2, lc1, lc2, m1, m2, g = p
Expand All @@ -88,7 +94,7 @@ _, x, p, double_pendulum_simplified = generate_control_function(
split=false
)

t = independent_variable(double_pendulum)
t, = independent_variables(double_pendulum)
Dt = Differential(t)

p = map(Base.Fix1(getproperty, double_pendulum), toexpr.(p))
Expand All @@ -109,15 +115,15 @@ double_pendulum_feedback_cancellation = structural_simplify(double_pendulum_feed

# Swing up to upward equilibrium
# Assume uniform rods of random mass and length
m1, m2 = rand(2)
l1, l2 = rand(2)
m1, m2 = ones(2)
l1, l2 = ones(2)
lc1, lc2 = l1 /2, l2 / 2
I1 = m1 * l1^2 / 3
I2 = m2 * l2^2 / 3
g = 1.0
p = Dict(p .=> [I1, I2, l1, l2, lc1, lc2, m1, m2, g])

x0 = Dict(x .=> vcat(2π * rand(2) .- π, rand(2)))
x0 = Dict(x .=> vcat(2π * rand(rng, 2) .- π, rand(rng, 2)))

prob = ODEProblem(double_pendulum_feedback_cancellation, x0, 100, p)
sol = solve(prob, Tsit5())
Expand All @@ -143,6 +149,8 @@ gif(
=#

################################## LQR acrobot controller ##################################
println("Acrobot LQR test")

function acrobot_lqr_matrix(p; x_eq = [π, 0, 0, 0], Q = I(4), R = I(1))
I1, I2, l1, l2, lc1, lc2, m1, m2, g = p
θ1, θ2 = x_eq[1:2]
Expand Down Expand Up @@ -178,7 +186,7 @@ _, x, params, acrobot_simplified = generate_control_function(
split=false
)

t = independent_variable(acrobot)
t, = independent_variables(acrobot)
Dt = Differential(t)

params = map(Base.Fix1(getproperty, acrobot), toexpr.(params))
Expand Down Expand Up @@ -207,15 +215,15 @@ p = [I1, I2, l1, l2, lc1, lc2, m1, m2, g]
acrobot_lqr = structural_simplify(acrobot_lqr)

# Remain close to upward equilibrium
x0 = [π, π, 0, 0] + 0.005 * vcat(2π * rand(2) .- π, 2 * rand(2) .- 1)
x0 = [π, π, 0, 0] + 0.005 * vcat(2π * rand(rng, 2) .- π, 2 * rand(rng, 2) .- 1)
tspan = 1000

prob = ODEProblem(acrobot_lqr, Dict(x .=> x0), tspan, Dict(params .=> p))
sol = solve(prob, Tsit5())

anim = plot_double_pendulum(sol, p; angle1_symbol=:acrobot₊θ1, angle2_symbol=:acrobot₊θ2)
@test anim isa Plots.Animation
gif(anim, fps=50)
# gif(anim, fps=50)

x1_end, y1_end, ω1_end = sin(sol[acrobot.θ1][end]), -cos(sol[acrobot.θ1][end]), sol[Dt(acrobot.θ1)][end]
x2_end, y2_end, ω2_end = sin(sol[acrobot.θ2][end]), -cos(sol[acrobot.θ2][end]), sol[Dt(acrobot.θ2)][end]
Expand Down
18 changes: 12 additions & 6 deletions lib/NeuralLyapunovProblemLibrary/test/pendulum_test.jl
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,15 @@ import ModelingToolkit: inputs, generate_control_function
using NeuralLyapunovProblemLibrary
using OrdinaryDiffEq
using Plots
using Test
using Test, StableRNGs

rng = StableRNG(0)

################## Undriven pendulum should drop to downward equilibrium ###################
x0 = π * rand(2)
p = rand(2)
println("Undriven pendulum test")

x0 = π * rand(rng, 2)
p = rand(rng, 2)
τ = 1 / prod(p)
prob = ODEProblem(structural_simplify(pendulum_undriven), x0, 15τ, p)
sol = solve(prob, Tsit5())
Expand All @@ -20,6 +24,8 @@ anim = plot_pendulum(sol)
# gif(anim, fps=50)

############################# Feedback cancellation controller #############################
println("Simple pendulum feedback cancellation test")

π_cancellation(x, p) = 2 * p[2]^2 * sin(x[1])

_, x, p, pendulum_simplified = generate_control_function(
Expand All @@ -28,7 +34,7 @@ _, x, p, pendulum_simplified = generate_control_function(
split=false
)

t = independent_variable(pendulum)
t, = independent_variables(pendulum)
Dt = Differential(t)

p = map(Base.Fix1(getproperty, pendulum), toexpr.(p))
Expand All @@ -47,8 +53,8 @@ u = map(
pendulum_feedback_cancellation = structural_simplify(pendulum_feedback_cancellation)

# Swing up to upward equilibrium
x0 = rand(2)
p = rand(2)
x0 = rand(rng, 2)
p = rand(rng, 2)
τ = 1 / prod(p)
prob = ODEProblem(pendulum_feedback_cancellation, x0, 15τ, p)
sol = solve(prob, Tsit5())
Expand Down
18 changes: 12 additions & 6 deletions lib/NeuralLyapunovProblemLibrary/test/planar_quadrotor_test.jl
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,13 @@ using OrdinaryDiffEq
using Plots
using LinearAlgebra
using ControlSystemsBase: lqr, Continuous
using Test
using Test, StableRNGs

rng = StableRNG(0)

#################################### Hovering quadrotor ####################################
println("Planar quadrotor vertical only test")

function π_vertical_only(x, p; y_goal=0.0, k_p=1.0, k_d=1.0)
y, ẏ = x[2], x[5]
m, I_quad, g, r = p
Expand All @@ -23,7 +27,7 @@ _, _, p, quadrotor_planar_simplified = generate_control_function(
split=false
)

t = independent_variable(quadrotor_planar)
t, = independent_variables(quadrotor_planar)
Dt = Differential(t)
q = setdiff(unknowns(quadrotor_planar), inputs(quadrotor_planar))

Expand Down Expand Up @@ -51,8 +55,8 @@ quadrotor_planar_vertical_only = structural_simplify(quadrotor_planar_vertical_o
# Hovering
# Assume rotors are negligible mass when calculating the moment of inertia
x0 = Dict(x .=> zeros(6))
x0[q[2]] = rand()
x0[x[5]] = rand()
x0[q[2]] = rand(rng)
x0[x[5]] = rand(rng)
m, r = ones(2)
g = 1.0
I_quad = m * r^2 / 12
Expand Down Expand Up @@ -84,6 +88,8 @@ anim = plot_quadrotor_planar(
# gif(anim, fps = 50)

############################## LQR planar quadrotor controller #############################
println("Planar quadrotor LQR test")

function quadrotor_planar_lqr_matrix(p; Q = I(6), R = I(2))
m, I_quad, g, r = p

Expand Down Expand Up @@ -113,7 +119,7 @@ _, _, p, quadrotor_planar_simplified = generate_control_function(
split=false
)

t = independent_variable(quadrotor_planar)
t, = independent_variables(quadrotor_planar)
Dt = Differential(t)
q = setdiff(unknowns(quadrotor_planar), inputs(quadrotor_planar))

Expand Down Expand Up @@ -145,7 +151,7 @@ p = [m, I_quad, g, r]
quadrotor_planar_lqr = structural_simplify(quadrotor_planar_lqr)

# Fly to origin
x0 = Dict(x .=> 2 * rand(6) .- 1)
x0 = Dict(x .=> 2 * rand(rng, 6) .- 1)
p = Dict(params .=> [m, I_quad, g, r])
τ = sqrt(r / g)

Expand Down
22 changes: 14 additions & 8 deletions lib/NeuralLyapunovProblemLibrary/test/quadrotor_test.jl
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,13 @@ using OrdinaryDiffEq
using Plots
using LinearAlgebra
using ControlSystemsBase: lqr, Continuous
using Test
using Test, StableRNGs

rng = StableRNG(0)

#################################### Hovering quadrotor ####################################
println("3D quadrotor vertical only test")

function π_vertical_only(x, p; z_goal=0.0, k_p=1.0, k_d=1.0)
z = x[3]
= x[9]
Expand All @@ -31,7 +35,7 @@ _, _, p, quadrotor_3d_simplified = generate_control_function(
split=false
)

t = independent_variable(quadrotor_3d)
t, = independent_variables(quadrotor_3d)
Dt = Differential(t)
x = setdiff(unknowns(quadrotor_3d), inputs(quadrotor_3d))

Expand Down Expand Up @@ -68,9 +72,9 @@ p = Dict(params .=> [m, g, Ixx, Ixy, Ixz, Iyy, Iyz, Izz])
τ = sqrt(L / g)

x0 = Dict(x .=> zeros(12))
x0[q[3]] = rand()
x0[q̇[3]] = rand()
x0[q̇[6]] = rand()
x0[q[3]] = rand(rng)
x0[q̇[3]] = rand(rng)
x0[q̇[6]] = rand(rng)

prob = ODEProblem(quadrotor_3d_vertical_only, x0, 15τ, p)
sol = solve(prob, Tsit5())
Expand Down Expand Up @@ -107,6 +111,8 @@ anim = plot_quadrotor_3d(
# gif(anim, fps = 50)

############################## LQR planar quadrotor controller #############################
println("3D quadrotor LQR test")

function quadrotor_3d_lqr_matrix(
p;
x_eq = zeros(12),
Expand Down Expand Up @@ -146,7 +152,7 @@ _, _, p, quadrotor_3d_simplified = generate_control_function(
split=false
)

t = independent_variable(quadrotor_3d)
t, = independent_variables(quadrotor_3d)
Dt = Differential(t)
x = setdiff(unknowns(quadrotor_3d), inputs(quadrotor_3d))

Expand Down Expand Up @@ -182,8 +188,8 @@ quadrotor_3d_lqr = structural_simplify(quadrotor_3d_lqr)
# Fly to origin
p = Dict(params .=> p)
δ = 0.5
x0 = Dict(x .=> δ .* (2 .* rand(12) .- 1))
τ = sqrt(r / g)
x0 = Dict(x .=> δ .* (2 .* rand(rng, 12) .- 1))
τ = sqrt(L / g)

prob = ODEProblem(quadrotor_3d_lqr, x0, 15τ, p)
sol = solve(prob, Tsit5())
Expand Down

0 comments on commit d744e09

Please sign in to comment.