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

@@ -1,7 +1,7 @@
 name = "FSimZoo"
 uuid = "e2d5807d-1594-43f7-91a6-54c6eef5a9d2"
 authors = ["JinraeKim <kjl950403@gmail.com> and contributors"]
-version = "0.9.0"
+version = "0.10.0"
 
 [deps]
 ComponentArrays = "b0b7db55-cfe3-40fc-9ded-d10e2dbeff66"

README.md

@@ -20,9 +20,16 @@ contains predefined environments and controllers for [FlightSims.jl](https://git
  </details>
 
  <details>
+ <summary>Actuators</summary>
+
+ - `SecondOrderActuator`
+
+ </details>
+ <details>
+
  <summary>fixed wings</summary>
 
- - `WingRock`
+ - (Wing Rock phenomenon) `TarnWingRock`, `ElzebdaWingRock`
 
  </details>
 

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

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

src/environments/actuators/actuators.jl

@@ -0,0 +1,4 @@
+abstract type AbstractActuator <: AbstractEnv end
+
+
+include("SecondOrderActuator.jl")

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")
 

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

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

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")