diff --git a/Project.toml b/Project.toml index 5e1edbb..33c86a4 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.6.0" +version = "0.7.0" [deps] ComponentArrays = "b0b7db55-cfe3-40fc-9ded-d10e2dbeff66" diff --git a/README.md b/README.md index 568bbcd..ec2715b 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,7 @@ contains predefined environments and controllers for [FlightSims.jl](https://git
multicopters - - (Hexacopter) `LeeHexacopter`, `LeeQuadcopter` (**currently maintained**) + - (Hexacopter) `LeeHexacopter`, `LeeQuadcopter`, `GoodarziAgileQuadcopter` (**currently maintained**) - (Quadcopter) `IslamQuadcopter`, `GoodarziQuadcopter`
diff --git a/src/FSimZoo.jl b/src/FSimZoo.jl index 144f33b..72e8308 100644 --- a/src/FSimZoo.jl +++ b/src/FSimZoo.jl @@ -20,7 +20,7 @@ export GeometricTrackingController, OuterLoopGeometricTrackingController, InnerL export InputAffinePositionCBF ## multicopters export Multicopter -export Quadcopter, IslamQuadcopter, GoodarziQuadcopter, LeeQuadcopter +export Quadcopter, IslamQuadcopter, GoodarziQuadcopter, LeeQuadcopter, GoodarziAgileQuadcopter export Hexacopter, LeeHexacopter # control allocator export AbstractAllocator, StaticAllocator diff --git a/src/environments/multicopters/GoodarziAgileQuadcopter.jl b/src/environments/multicopters/GoodarziAgileQuadcopter.jl new file mode 100644 index 0000000..610ab4d --- /dev/null +++ b/src/environments/multicopters/GoodarziAgileQuadcopter.jl @@ -0,0 +1,43 @@ +""" +# Notes +- [1] (quad-+ configuration); see [2]. + +# References +[1] F. A. Goodarzi, D. Lee, and T. Lee, “Geometric Adaptive Tracking Control of a Quadrotor Unmanned Aerial Vehicle on SE(3) for Agile Maneuvers,” Journal of Dynamic Systems, Measurement, and Control, vol. 137, no. 9, p. 091007, Sep. 2015, doi: 10.1115/1.4030419. +[2] PX4 Airframes Reference, https://docs.px4.io/master/en/airframes/airframe_reference.html. +# Variables +u ∈ R^4: rotor forces +""" +Base.@kwdef struct GoodarziAgileQuadcopter <: Quadcopter + J = 1e-2 * [ + 5.5711 0.0618 -0.0251; + 0.06177 5.5757 0.0101; + -0.02502 0.01007 1.05053; + ] # kg m^2 + l = 0.169 # m + kM = 0.1056 # m + m = 0.755 # kg + g = 9.81 # m / s^2 + B = [ 1 1 1 1; + -l l 0 0; + 0 0 l -l; + kM kM -kM -kM] + # actuator limit + dim_input = 4 + u_min = zeros(dim_input) + u_max = 3.2 * ones(dim_input) +end + +function saturate(multicopter::GoodarziAgileQuadcopter, u) + (; u_min, u_max) = multicopter + u_saturated = clamp.(u, u_min, u_max) +end + +function input_to_force_moment(multicopter::GoodarziAgileQuadcopter, u) + (; B) = multicopter + ν = B * u +end + +function airframe_reference(multicopter::GoodarziAgileQuadcopter) + :quad_plus +end diff --git a/src/environments/multicopters/quadcopters.jl b/src/environments/multicopters/quadcopters.jl index b589a25..cf50d45 100644 --- a/src/environments/multicopters/quadcopters.jl +++ b/src/environments/multicopters/quadcopters.jl @@ -3,3 +3,4 @@ abstract type Quadcopter <: Multicopter end include("GoodarziQuadcopter.jl") include("IslamQuadcopter.jl") include("LeeQuadcopter.jl") +include("GoodarziAgileQuadcopter.jl")