diff --git a/lib/NeuralLyapunovProblemLibrary/Project.toml b/lib/NeuralLyapunovProblemLibrary/Project.toml index 7dbab0e..4711cf6 100644 --- a/lib/NeuralLyapunovProblemLibrary/Project.toml +++ b/lib/NeuralLyapunovProblemLibrary/Project.toml @@ -22,6 +22,7 @@ OrdinaryDiffEq = "6.91.0" Plots = "1.40.9" Rotations = "1.7.1" SafeTestsets = "0.1.0" +StableRNGs = "1.0.2" julia = "1.11" [extras] @@ -29,7 +30,8 @@ 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"] diff --git a/lib/NeuralLyapunovProblemLibrary/src/quadrotor.jl b/lib/NeuralLyapunovProblemLibrary/src/quadrotor.jl index 80b7221..2293cff 100644 --- a/lib/NeuralLyapunovProblemLibrary/src/quadrotor.jl +++ b/lib/NeuralLyapunovProblemLibrary/src/quadrotor.jl @@ -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) @@ -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 f̃ = max.(0, f) T̃ = [1 1 1 1; 0 1 0 -1; -1 0 1 0; -1 1 -1 1] * f̃ diff --git a/lib/NeuralLyapunovProblemLibrary/test/double_pendulum_test.jl b/lib/NeuralLyapunovProblemLibrary/test/double_pendulum_test.jl index c9d414c..a879812 100644 --- a/lib/NeuralLyapunovProblemLibrary/test/double_pendulum_test.jl +++ b/lib/NeuralLyapunovProblemLibrary/test/double_pendulum_test.jl @@ -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) @@ -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) @@ -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 @@ -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)) @@ -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()) @@ -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] @@ -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)) @@ -207,7 +215,7 @@ 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)) @@ -215,7 +223,7 @@ 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] diff --git a/lib/NeuralLyapunovProblemLibrary/test/pendulum_test.jl b/lib/NeuralLyapunovProblemLibrary/test/pendulum_test.jl index 261bef8..25d0ce0 100644 --- a/lib/NeuralLyapunovProblemLibrary/test/pendulum_test.jl +++ b/lib/NeuralLyapunovProblemLibrary/test/pendulum_test.jl @@ -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()) @@ -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( @@ -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)) @@ -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()) diff --git a/lib/NeuralLyapunovProblemLibrary/test/planar_quadrotor_test.jl b/lib/NeuralLyapunovProblemLibrary/test/planar_quadrotor_test.jl index 79dffdd..dc8934a 100644 --- a/lib/NeuralLyapunovProblemLibrary/test/planar_quadrotor_test.jl +++ b/lib/NeuralLyapunovProblemLibrary/test/planar_quadrotor_test.jl @@ -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 @@ -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)) @@ -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 @@ -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 @@ -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)) @@ -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) diff --git a/lib/NeuralLyapunovProblemLibrary/test/quadrotor_test.jl b/lib/NeuralLyapunovProblemLibrary/test/quadrotor_test.jl index 220846d..fb236e5 100644 --- a/lib/NeuralLyapunovProblemLibrary/test/quadrotor_test.jl +++ b/lib/NeuralLyapunovProblemLibrary/test/quadrotor_test.jl @@ -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] @@ -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)) @@ -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()) @@ -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), @@ -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)) @@ -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())