From c383ffe6803b6b5a1b740e7350394d510d950685 Mon Sep 17 00:00:00 2001 From: Jinrae Kim Date: Mon, 15 May 2023 19:49:36 +0900 Subject: [PATCH] Feature/oop wingrock dynamics (#23) * Add out-of-place wingrock dynamics * Add wingrock models * Bump a new version * Update READMe * Adding SecondOrderActuator * Fix a test bug * Add SecondOrderActuator * Add logging * Change oop_dynamics to Dynamics * Update README.md * Fix bugs * Fix a test bug * Add MissileLongitudinal * Add MissileLongitudinal * Fix a minor bug * Remove previously developed missiles --- Project.toml | 2 +- README.md | 9 +- src/FSimZoo.jl | 6 +- .../actuators/SecondOrderActuator.jl | 23 ++++ src/environments/actuators/actuators.jl | 4 + src/environments/environments.jl | 6 + .../fixedwings/ElzebdaWingRock.jl | 61 ++++++++++ src/environments/fixedwings/TarnWingRock.jl | 64 ++++++++++ src/environments/fixedwings/wingrock.jl | 40 +------ .../missiles.bak/PointMass3DMissile.jl | 22 ---- .../missiles.bak/PursuerEvador3DMissile.jl | 20 ---- src/environments/missiles.bak/missiles.jl | 4 - .../missiles/MissileLongitudinal.jl | 110 ++++++++++++++++++ src/environments/missiles/missiles.jl | 1 + .../actuators/SecondOrderActuator.jl | 21 ++++ test/environments/actuators/actuators.jl | 1 + test/environments/environments.jl | 2 + test/environments/fixedwings/wingrock.jl | 22 +++- .../missiles/MissileLongitudinal.jl | 18 +++ test/environments/missiles/missiles.jl | 1 + 20 files changed, 348 insertions(+), 89 deletions(-) create mode 100644 src/environments/actuators/SecondOrderActuator.jl create mode 100644 src/environments/actuators/actuators.jl create mode 100644 src/environments/fixedwings/ElzebdaWingRock.jl create mode 100644 src/environments/fixedwings/TarnWingRock.jl delete mode 100644 src/environments/missiles.bak/PointMass3DMissile.jl delete mode 100644 src/environments/missiles.bak/PursuerEvador3DMissile.jl delete mode 100644 src/environments/missiles.bak/missiles.jl create mode 100644 src/environments/missiles/MissileLongitudinal.jl create mode 100644 src/environments/missiles/missiles.jl create mode 100644 test/environments/actuators/SecondOrderActuator.jl create mode 100644 test/environments/actuators/actuators.jl create mode 100644 test/environments/missiles/MissileLongitudinal.jl create mode 100644 test/environments/missiles/missiles.jl diff --git a/Project.toml b/Project.toml index b19fbf5..a32eebd 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "FSimZoo" uuid = "e2d5807d-1594-43f7-91a6-54c6eef5a9d2" authors = ["JinraeKim and contributors"] -version = "0.9.0" +version = "0.10.0" [deps] ComponentArrays = "b0b7db55-cfe3-40fc-9ded-d10e2dbeff66" diff --git a/README.md b/README.md index 6f01076..63ed367 100644 --- a/README.md +++ b/README.md @@ -20,9 +20,16 @@ contains predefined environments and controllers for [FlightSims.jl](https://git
+ Actuators + + - `SecondOrderActuator` + +
+
+ fixed wings - - `WingRock` + - (Wing Rock phenomenon) `TarnWingRock`, `ElzebdaWingRock`
diff --git a/src/FSimZoo.jl b/src/FSimZoo.jl index 9ac3cd1..6a6872b 100644 --- a/src/FSimZoo.jl +++ b/src/FSimZoo.jl @@ -18,12 +18,16 @@ export TwoDimensionalNonlinearDTSystem export LQR, PID, BacksteppingPositionController export GeometricTrackingController, OuterLoopGeometricTrackingController, InnerLoopGeometricTrackingController export InputAffinePositionCBF +## actuators +export SecondOrderActuator ## fixedwings -export WingRock +export AbstractWingRock, ElzebdaWingRock, TarnWingRock ## multicopters export Multicopter export Quadcopter, IslamQuadcopter, GoodarziQuadcopter, LeeQuadcopter, GoodarziAgileQuadcopter export Hexacopter, LeeHexacopter +## missiles +export MissileLongitudinal # control allocator export AbstractAllocator, StaticAllocator export PseudoInverseAllocator diff --git a/src/environments/actuators/SecondOrderActuator.jl b/src/environments/actuators/SecondOrderActuator.jl new file mode 100644 index 0000000..6c67175 --- /dev/null +++ b/src/environments/actuators/SecondOrderActuator.jl @@ -0,0 +1,23 @@ +struct SecondOrderActuator <: AbstractActuator + ζ + ω +end + + +function State(env::SecondOrderActuator) + return function (δ, δ̇) + ComponentArray(δ=δ, δ̇=δ̇) + end +end + + +function Dynamics!(env::SecondOrderActuator) + (; ζ, ω) = env + @Loggable function dynamics!(dx, x, p, t; δ_cmd) + (; δ, δ̇) = x + @log state = x + @log input = δ_cmd + dx.δ = δ̇ + dx.δ̇ = -2*ζ*ω*δ̇ + ω^2*(δ_cmd-δ) + end +end diff --git a/src/environments/actuators/actuators.jl b/src/environments/actuators/actuators.jl new file mode 100644 index 0000000..cc27747 --- /dev/null +++ b/src/environments/actuators/actuators.jl @@ -0,0 +1,4 @@ +abstract type AbstractActuator <: AbstractEnv end + + +include("SecondOrderActuator.jl") diff --git a/src/environments/environments.jl b/src/environments/environments.jl index d9efb85..befd18f 100644 --- a/src/environments/environments.jl +++ b/src/environments/environments.jl @@ -2,12 +2,18 @@ ## Basic environments include("basics/basics.jl") +## Actuators +include("actuators/actuators.jl") + ## Fixed wings include("fixedwings/fixedwings.jl") ## Multicopters include("multicopters/multicopters.jl") +## Missiles +include("missiles/missiles.jl") + ## Control Allocators include("allocators/allocators.jl") diff --git a/src/environments/fixedwings/ElzebdaWingRock.jl b/src/environments/fixedwings/ElzebdaWingRock.jl new file mode 100644 index 0000000..d050581 --- /dev/null +++ b/src/environments/fixedwings/ElzebdaWingRock.jl @@ -0,0 +1,61 @@ +""" +Wing-rock phenomenon in the roll motion of slendar delta wings [1]. + +# Notes +## Nondimensional values +According to [2], the time, state, and control input of this model are nondimensional. +But here, it is used as time [s], state [rad, rad/s]. + +## Input constraint +Input constraint in [3] is used. + +## Ideal parameters +The default ideal parameters, W_true, are set as the specific model obtained at the angle of attack of 25 degrees in [1]. +Note that the ideal parameters in [2] are (1000 x the ideal parameters in [1]). + +# Refs +[1] J. M. Elzebda, A. H. Nayfeh, and D. T. Mook, “Development of an analyt- ical model of wing rock for slender delta wings,” J. Aircr., vol. 26, no. 8, pp. 737–743, 1989. +[2] N. Cho, H.-S. Shin, Y. Kim, and A. Tsourdos, “Composite Model Reference Adaptive Control with Parameter Convergence Under Finite Excitation,” IEEE Trans. Automat. Contr., vol. 63, no. 3, pp. 811–818, Mar. 2018, doi: 10.1109/TAC.2017.2737324. +[3] J. H. Tarn and F. Y. Hsu, “Fuzzy Control of Wing Rock for Slender Delta wings,” in 1993 American Control Conference, San Francisco, CA, USA: IEEE, Jun. 1993, pp. 1159–1161. doi: 10.23919/ACC.1993.4793048. +""" +Base.@kwdef struct ElzebdaWingRock <: AbstractWingRock + W_true = 1e-3 * [-18.59521, 15.162375, -62.45153, 9.54708, 21.45291] + u_min=-1.75 + u_max=+1.75 +end + + +function State(env::ElzebdaWingRock) + return function (x1=0.0, x2=0.0) + ComponentArray(x1=x1, x2=x2) + end +end + + +""" +Out-of-place dynamics +""" +function Dynamics(env::ElzebdaWingRock) + (; W_true) = env + return function (x, p, t; u) + (; x1, x2) = x + basis = [x1, x2, abs(x1)*x2, abs(x2)*x2, x1^3] + Δ = W_true' * basis + dx1 = x2 + dx2 = u + Δ + dx = State(env)(dx1, dx2) + return dx + end +end + + +function Dynamics!(env::ElzebdaWingRock) + (; u_min, u_max) = env + @Loggable function dynamics!(dx, x, p, t; u) + (; x1, x2) = x + u_saturated = clamp.(u, u_min, u_max) + @log state = x + @log input = ComponentArray(u_cmd=u, u_applied=u_saturated) + dx .= Dynamics(env)(x, p, t; u=u_saturated) + end +end diff --git a/src/environments/fixedwings/TarnWingRock.jl b/src/environments/fixedwings/TarnWingRock.jl new file mode 100644 index 0000000..5f0837f --- /dev/null +++ b/src/environments/fixedwings/TarnWingRock.jl @@ -0,0 +1,64 @@ +""" +Wing-rock phenomenon in the roll motion of slendar delta wings at AoA=25 deg [1]. + +# Notes +x = [x1, x2]^T +x1 = ϕ (roll angle) +x2 = ϕ̇ (roll rate) + +# Refs +[1] J. H. Tarn and F. Y. Hsu, “Fuzzy Control of Wing Rock for Slender Delta wings,” in 1993 American Control Conference, San Francisco, CA, USA: IEEE, Jun. 1993, pp. 1159–1161. doi: 10.23919/ACC.1993.4793048. +""" +Base.@kwdef struct TarnWingRock <: AbstractWingRock + a1=-0.05686 + a2=+0.03254 + a3=+0.07334 + a4=-0.35970 + a5=+1.46810 + C1=0.354 + C2=0.001 + u_min=-1.75 + u_max=+1.75 +end + + +function State(env::TarnWingRock) + return function (x1=0.0, x2=0.0) + ComponentArray(x1=x1, x2=x2) + end +end + + +""" +Out-of-place dynamics +""" +function Dynamics(env::TarnWingRock) + (; a1, a2, a3, a4, a5, C1, C2) = env + return function (x, p, t; u) + (; x1, x2) = x + ω_squares = -C1*a1 + μ1 = C1*a2 - C2 + b1 = C1*a3 + μ2 = C1*a4 + b2 = C1*a5 + basis = [x1, x2, x1^2*x2, x1*x2^2, x1^3] + coeff = [-ω_squares, μ1, μ2, b2, b1] + Δ = coeff' * basis + dx1 = x2 + dx2 = u + Δ + dx = State(env)(dx1, dx2) + return dx + end +end + + +function Dynamics!(env::TarnWingRock) + (; u_min, u_max) = env + @Loggable function dynamics!(dx, x, p, t; u) + (; x1, x2) = x + u_saturated = clamp.(u, u_min, u_max) + @log state = x + @log input = ComponentArray(u_cmd=u, u_applied=u_saturated) + dx .= Dynamics(env)(x, p, t; u=u_saturated) + end +end diff --git a/src/environments/fixedwings/wingrock.jl b/src/environments/fixedwings/wingrock.jl index dc9c639..929d712 100644 --- a/src/environments/fixedwings/wingrock.jl +++ b/src/environments/fixedwings/wingrock.jl @@ -1,39 +1,5 @@ -""" -Wing-rock phenomenon in the roll motion of slendar delta wings [1]. +abstract type AbstractWingRock <: AbstractEnv end -# Notes -## Nondimensional values -The time, state, and control input of this model are nondimensional [2]. -## Ideal parameters -The default ideal parameters, W_true, are set as the specific model obtained at the angle of attack of 25 degrees in [1]. -Note that the ideal parameters in [2] are (1000 x the ideal parameters in [1]). - -# Refs -[1] J. M. Elzebda, A. H. Nayfeh, and D. T. Mook, “Development of an analyt- ical model of wing rock for slender delta wings,” J. Aircr., vol. 26, no. 8, pp. 737–743, 1989. -[2] N. Cho, H.-S. Shin, Y. Kim, and A. Tsourdos, “Composite Model Reference Adaptive Control with Parameter Convergence Under Finite Excitation,” IEEE Trans. Automat. Contr., vol. 63, no. 3, pp. 811–818, Mar. 2018, doi: 10.1109/TAC.2017.2737324. -""" -Base.@kwdef struct WingRock <: AbstractEnv - W_true = 1e-3 * [-18.59521, 15.162375, -62.45153, 9.54708, 21.45291] -end - - -function State(env::WingRock) - return function (x1=0.0, x2=0.0) - ComponentArray(x1=x1, x2=x2) - end -end - - -function Dynamics!(env::WingRock) - (; W_true) = env - @Loggable function dynamics!(dx, x, p, t; u) - (; x1, x2) = x - @log state = x - @log input = u - ϕ = [x1, x2, abs(x1)*x2, abs(x2)*x2, x1^3] - Δ = W_true' * ϕ - dx.x1 = x2 - dx.x2 = u + Δ - end -end +include("ElzebdaWingRock.jl") +include("TarnWingRock.jl") diff --git a/src/environments/missiles.bak/PointMass3DMissile.jl b/src/environments/missiles.bak/PointMass3DMissile.jl deleted file mode 100644 index 4ca216a..0000000 --- a/src/environments/missiles.bak/PointMass3DMissile.jl +++ /dev/null @@ -1,22 +0,0 @@ -struct PointMass3DMissile <: AbstractMissile -end - -function State(env::PointMass3DMissile) - return function (p=zeros(3), v=zeros(3)) - ComponentArray(p=p, v=v) - end -end - - -""" -u ∈ ℝ³: acceleration -""" -function Dynamics!(env::PointMass3DMissile) - @Loggable function dynamics!(dx, x, params, t; u) - @unpack p, v = x - @log p - @log v - dx.p = v - dx.v = u - end -end diff --git a/src/environments/missiles.bak/PursuerEvador3DMissile.jl b/src/environments/missiles.bak/PursuerEvador3DMissile.jl deleted file mode 100644 index e03fce0..0000000 --- a/src/environments/missiles.bak/PursuerEvador3DMissile.jl +++ /dev/null @@ -1,20 +0,0 @@ -struct PursuerEvador3DMissile <: AbstractMissile - pursuer::PointMass3DMissile - evador::PointMass3DMissile -end - -function State(env::PursuerEvador3DMissile) - @unpack pursuer, evador = env - return function (x0_pursuer, x0_evador) - ComponentArray(pursuer=x0_pursuer, evador=x0_evador) - end -end - -function Dynamics!(env::PursuerEvador3DMissile) - @unpack pursuer, evador = env - @Loggable function dynamics!(dx, x, params, t; u_pursuer, u_evador) - @onlylog r = norm(x.pursuer.p - x.evador.p) - @nested_log :pursuer Dynamics!(pursuer)(dx.pursuer, x.pursuer, nothing, t; u=u_pursuer) - @nested_log :evador Dynamics!(evador)(dx.evador, x.evador, nothing, t; u=u_evador) - end -end diff --git a/src/environments/missiles.bak/missiles.jl b/src/environments/missiles.bak/missiles.jl deleted file mode 100644 index 499de8e..0000000 --- a/src/environments/missiles.bak/missiles.jl +++ /dev/null @@ -1,4 +0,0 @@ -abstract type AbstractMissile <: AbstractEnv end - -include("PointMass3DMissile.jl") -include("PursuerEvador3DMissile.jl") diff --git a/src/environments/missiles/MissileLongitudinal.jl b/src/environments/missiles/MissileLongitudinal.jl new file mode 100644 index 0000000..64f7009 --- /dev/null +++ b/src/environments/missiles/MissileLongitudinal.jl @@ -0,0 +1,110 @@ +""" +# Refs +[1] C. Mracek and J. Cloutier, “Full Envelope Missile Longitudinal Autopilot Design Using the State-Dependent Riccati Equation Method,” Guidance, Navigation, and Control Conference, Aug. 1997, pp. 1697-1705, doi:10.2514/6.1997-3767. +""" +Base.@kwdef struct MissileLongitudinal + grav = 9.81 + # Physical Configuration (Table 3) + S_ref = 0.0409 # Reference Area [m] (0.44 ft^2) + d_ref = 0.2286 # Reference Distance [m] (0.75 ft) + mass = 204.02 # Mass [kg] (13.98 slug) + I_yy = 247.438 # Moment of Inertia [kg * m^2] (182.5 sulg-ft^2) + + # Aerodynamic coefficients (Table 1 in Ref.) + aₐ = 0.3 + aₘ = 40.44 + aₙ = 19.373 + bₘ = -64.015 + bₙ = -31.023 + cₘ = 2.922 + cₙ = -9.717 + dₘ = -11.803 + dₙ = -1.948 + eₘ = -1.719 +end + + +""" +α: Angle of attack [rad] +q : Pitch rate [rad/s] +mach : Mach number -] +γ : Flight path angle [rad] +""" +function State(env::MissileLongitudinal) + return function (α=deg2rad(0), q=deg2rad(0), M=4.0, γ=deg2rad(0), h=10_000) + ComponentArray(α=α, q=q, M=M, γ=γ, h=h) + end +end + + +function density(h) + # Atmospheric coefficients (Table 2 in Ref.) + tro_alt = 10972.8 # troposphere boundary [m] (36000.0 ft) + str_alt = 20116.8 # stratosphere boundary [m] (66000.0 ft) + ρ₀sl = 1.1455 # Air density at sea level [kg/m^3] (2.377e-3 lb-s^2/ft^2) + ρ₀tr = 0.3415 # Air density at troposphere boundary [kg/m^3] (0.7086e-3 lb-s^2/ft^2) + Kρsl = 1.1103e-4 # [1/m] (3.36174e-5 1/ft) + Kρtr = 1.5760e-4 # [1/m] (4.80377e-5 1/ft) + + @assert h >= 0.0 #, "Altitude must be greater than zero" + @assert h <= str_alt #, "Out of operating altitude range" + if h <= tro_alt + return ρ₀sl * exp(-Kρsl * h) + else + return ρ₀tr * exp(-Kρtr*(h - tro_alt)) + end +end + + +function sonic_speed(h) + # Atmospheric coefficients (Ttable 2 in Ref.) + tro_alt = 10972.8 # troposphere boundary [m] (36000.0 ft) + str_alt = 20116.8 # stratosphere boundary [m] (66000.0 ft) + asl = 340.2787 # Sonic speed at the sea level [m/s] (1116.4 ft/sec) + atr = 295.0769 # Sonic speed at the troposphere boundary [m/s] (968.1 ft/sec) + Ka = 1.0358e-2 # [1/m] (0.00410833 1/ft) + + @assert h >= 0.0 #, "Altitude must be greater than zero" + @assert h <= str_alt #, "Out of operating altitude range" + if h <= tro_alt + return asl - Ka * h + else + return atr + end +end + + +function Dynamics(env::MissileLongitudinal) + (; + grav, S_ref, d_ref, mass, I_yy, + aₐ, aₘ, aₙ, bₘ, bₙ, cₘ, cₙ, dₘ, dₙ, eₘ, + ) = env + + function (x, params, t; δ) + (; α, q, M, γ, h) = x + + Ca() = aₐ + Cz(α, M, δ) = (aₙ*α^3 + bₙ* α * abs(α) + cₙ * (2.0 - M/3.0)*α +dₙ *δ) + Cm(α, M, δ, q) = (aₘ*α^3 + bₘ* α * abs(α) + cₘ * (-7.0 + 8.0*M/3.0) *α +dₘ *δ + eₘ*q) + + Vₛ = sonic_speed(h) + ρ = density(h) + + α_dot = ρ * Vₛ * M * S_ref / (2.0 * mass) * (Cz(α, M, δ) * cos(α) - Ca() * sin(α)) + grav/(Vₛ * M) *cos(γ)+ q + q_dot = ρ * Vₛ^2 * M^2 * S_ref * d_ref / (2.0 * I_yy) * Cm(α, M, δ, q) + M_dot = ρ * Vₛ * M^2 * S_ref / (2.0 * mass) * (Cz(α, M, δ) * sin(α) + Ca() * cos(α)) - grav/Vₛ * sin(γ) + γ_dot = -ρ * Vₛ * M * S_ref / (2.0 * mass) * (Cz(α, M, δ) * cos(α) - Ca() * sin(α)) - grav/(Vₛ * M) * cos(γ) + h_dot = M * Vₛ * sin(γ) + dx = [α_dot, q_dot, M_dot, γ_dot, h_dot] + end +end + + +function Dynamics!(env::MissileLongitudinal) + dynamics = Dynamics(env) + @Loggable function dynamics!(dx, x, params, t; δ) + @log state = x + @log input = δ + dx .= dynamics(x, params, t; δ=δ) + end +end diff --git a/src/environments/missiles/missiles.jl b/src/environments/missiles/missiles.jl new file mode 100644 index 0000000..d7c8e48 --- /dev/null +++ b/src/environments/missiles/missiles.jl @@ -0,0 +1 @@ +include("MissileLongitudinal.jl") diff --git a/test/environments/actuators/SecondOrderActuator.jl b/test/environments/actuators/SecondOrderActuator.jl new file mode 100644 index 0000000..718a2dc --- /dev/null +++ b/test/environments/actuators/SecondOrderActuator.jl @@ -0,0 +1,21 @@ +using FSimZoo +using Test +using FSimBase + + +function main() + t = 0.0 + ζ = 0.7 + ω = 150 + env = SecondOrderActuator(ζ, ω) + x1, x2 = rand(2) + X = State(env)(x1, x2) + δ_cmd = 0.0 + dX = deepcopy(X) + Dynamics!(env)(dX, X, [], t; δ_cmd=δ_cmd) +end + + +@testset "SecondOrderActuator" begin + main() +end diff --git a/test/environments/actuators/actuators.jl b/test/environments/actuators/actuators.jl new file mode 100644 index 0000000..80bdefc --- /dev/null +++ b/test/environments/actuators/actuators.jl @@ -0,0 +1 @@ +include("SecondOrderActuator.jl") diff --git a/test/environments/environments.jl b/test/environments/environments.jl index d71457c..f3a86af 100644 --- a/test/environments/environments.jl +++ b/test/environments/environments.jl @@ -1 +1,3 @@ include("fixedwings/fixedwings.jl") +include("actuators/actuators.jl") +include("missiles/missiles.jl") diff --git a/test/environments/fixedwings/wingrock.jl b/test/environments/fixedwings/wingrock.jl index d56076e..88ff4e1 100644 --- a/test/environments/fixedwings/wingrock.jl +++ b/test/environments/fixedwings/wingrock.jl @@ -3,10 +3,9 @@ using Test using FSimBase -function main() +function test_elzebdawingrock() t = 0.0 - W_true = rand(5) - env = WingRock(W_true) + env = ElzebdaWingRock() x1, x2 = rand(2) X = State(env)(x1, x2) u = 0.0 @@ -15,6 +14,23 @@ function main() end +function test_tarnwingrock() + t = 0.0 + env = TarnWingRock() + x1, x2 = deg2rad.(10*rand(2)) + X = State(env)(x1, x2) + u = 0.0 + dX = deepcopy(X) + Dynamics!(env)(dX, X, [], t; u=u) +end + + +function main() + test_elzebdawingrock() + test_tarnwingrock() +end + + @testset "wingrock" begin main() end diff --git a/test/environments/missiles/MissileLongitudinal.jl b/test/environments/missiles/MissileLongitudinal.jl new file mode 100644 index 0000000..4f8bb21 --- /dev/null +++ b/test/environments/missiles/MissileLongitudinal.jl @@ -0,0 +1,18 @@ +using FSimZoo +using Test +using FSimBase + + +function main() + env = MissileLongitudinal() + t = 0.0 + X = State(env)() + δ = deg2rad(10) + dX = deepcopy(X) + Dynamics!(env)(dX, X, [], t; δ=δ) +end + + +@testset "MissileLongitudinal" begin + main() +end diff --git a/test/environments/missiles/missiles.jl b/test/environments/missiles/missiles.jl new file mode 100644 index 0000000..d7c8e48 --- /dev/null +++ b/test/environments/missiles/missiles.jl @@ -0,0 +1 @@ +include("MissileLongitudinal.jl")