├── .gitignore ├── LICENSE ├── README.md ├── examples ├── cd_beta.png ├── coefficients.png └── render.png ├── pyfly ├── __init__.py ├── dryden.py ├── pid_controller.py ├── pyfly.py ├── pyfly_config.json └── x8_param.mat ├── setup.cfg └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | MANIFEST 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | .pytest_cache/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | db.sqlite3 58 | 59 | # Flask stuff: 60 | instance/ 61 | .webassets-cache 62 | 63 | # Scrapy stuff: 64 | .scrapy 65 | 66 | # Sphinx documentation 67 | docs/_build/ 68 | 69 | # PyBuilder 70 | target/ 71 | 72 | # Jupyter Notebook 73 | .ipynb_checkpoints 74 | 75 | # pyenv 76 | .python-version 77 | 78 | # celery beat schedule file 79 | celerybeat-schedule 80 | 81 | # SageMath parsed files 82 | *.sage.py 83 | 84 | # Environments 85 | .env 86 | .venv 87 | env/ 88 | venv/ 89 | ENV/ 90 | env.bak/ 91 | venv.bak/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # PyCharm 98 | .idea 99 | 100 | # Rope project settings 101 | .ropeproject 102 | 103 | # mkdocs documentation 104 | /site 105 | 106 | # mypy 107 | .mypy_cache/ 108 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 eivindeb 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PyFly - Python Fixed Wing Flight Simulator 2 | PyFly is a python implementation of a 6 DOF aerodynamic model for fixed wing aircraft. The base aircraft parameters must be specified through a parameter file, an example of such a file that is experimentally verified 3 | by wind tunnel testing is included for the Skywalker X8 UAV, courtesy of . PyFly uses 4 | quaternions internally for performance reasons and to avoid singularities, while constraints and initial conditions can be 5 | specified in euler angles for convenience. PyFly also simulates the effects of wind and stochastic turbulence, 6 | modeled with the Dryden turbulence model. A simple PID-controller tuned to the Skywalker X8 UAV is included for convenience 7 | and sanity checks. The [gym environment wrapper](https://github.com/eivindeb/fixed-wing-gym) further extends the functionality of the 8 | PyFly simulator and allows for integration with reinforcement learning libraries. 9 | 10 | Aerodynamic coefficients in PyFly contain nonlinear extensions in angle of attack and sideslip angle, designed with 11 | Newtonian flat-plate-theory, in an effort to extend model-validity in the state space and incorporate effects such as stall: 12 | 13 | ![AoA coefficients](examples/coefficients.png "Angle of attack")![Sideslip coefficients](examples/cd_beta.png "sideslip angle") 14 | 15 | ## Example 16 | 17 | ```python 18 | from pyfly import PyFly 19 | from pid_controller import PIDController 20 | 21 | sim = PyFly("pyfly_config.json", "x8_param.mat") 22 | sim.seed(0) 23 | 24 | sim.reset(state={"roll": -0.5, "pitch": 0.15}) 25 | 26 | pid = PIDController(sim.dt) 27 | pid.set_reference(phi=0.2, theta=0, va=22) 28 | 29 | for step_i in range(500): 30 | phi = sim.state["roll"].value 31 | theta = sim.state["pitch"].value 32 | Va = sim.state["Va"].value 33 | omega = [sim.state["omega_p"].value, sim.state["omega_q"].value, sim.state["omega_r"].value] 34 | 35 | action = pid.get_action(phi, theta, Va, omega) 36 | success, step_info = sim.step(action) 37 | 38 | if not success: 39 | break 40 | 41 | sim.render(block=True) 42 | ``` 43 | 44 | Rendering this scenario produces: 45 | 46 | ![Render result](examples/render.png "render result") 47 | 48 | ## Installation 49 | PyFly is available through PyPI at pyfly-fixed-wing: 50 | ```shell 51 | pip install pyfly-fixed-wing 52 | ``` 53 | Or, it can be installed from source: 54 | ```shell 55 | git clone https://github.com/eivindeb/pyfly 56 | cd pyfly 57 | pip install -e . 58 | ``` 59 | 60 | ## Documentation 61 | PyFly is highly configurable through its config json file. For a description of functions and their use, see the 62 | docstrings of each function. 63 | 64 | The config file consists of four blocks: 65 | 66 | ### System settings 67 | The system settings consists of the following arguments, of which all are required: 68 | 69 | * **dt**: Float. The integration duration for each call to step() 70 | * **g**: Float. The gravitational acceleration 71 | * **rho**: Float. The permutativity of the air 72 | * **turbulence** Boolean. Controls if turbulence (from Dryden Turbulence Model) is enabled. 73 | * **turbulence_intensity** String. If turbulence is enabled, controls the intensity of the turbulence as described in the Dryden 74 | Turbulence Model. One of "light", "moderate", "severe". 75 | * **turbulunce_sim_length** Int. How many steps that are simulated for each call to the Dryden Turbulence Model. 76 | Higher values gives more upfront computation cost but better overall performance for longer simulations. 77 | 78 | ### States 79 | All states used in the simulator must be declared in the states block. The simulator wont run without the states in 80 | PyFly.REQUIRED_VARIABLES being declared, but optional states such as energy states can be declared here if they are to 81 | be plotted or otherwise are needed. Quaternions are used internally to represent attitude, but initial/value/constraint 82 | conditions are given on the Euler angles roll, pitch and yaw. 83 | 84 | Only the name argument is strictly required, although other arguments can be 85 | required if the state is to be plotted. 86 | 87 | * **name**: String. Name of state 88 | * **unit**: String. The unit the variable is to be plotted in. Can be any arbitrary string or %, for which its values 89 | will be plotted as a percentage of maximum and minimum values as specified by value_max and value_min. States in plots 90 | that share units are drawn on the same axis. 91 | * **init_min**: Float. The minimum value the state can be initialized to. 92 | * **init_max**: Float. The maximum value the state can be initialized to. 93 | * **value_min**: Float. The minimum value the state can assume, e.g. set by physical constraints on state. 94 | * **value_max**: Float. The maximum value the state can assume, e.g. set by physical constraints on state. 95 | * **constraints_min** Float. The state will raise a ConstraintException if it is set lower than this value. 96 | * **constraints_min** Float. The state will raise a ConstraintException if it is set higher than this value. 97 | * **convert_to_radians** Boolean. Set this to true if the configuration arguments for the state are given in degrees. 98 | * **wrap** Boolean. If set to true, the value will be wrapped in [-pi, pi]. 99 | * **label** String. The label of the state in plots. Supports Latex syntax. 100 | 101 | For states representing actuators, some additional arguments are required: 102 | * **order** Integer. The order of the dynamics of the actuator state. One of 1 or 2. 103 | * **omega_0** Float. The undamped natural frequency of the second order dynamics. 104 | * **zeta** Float. The damping factor of the second order dynamics. 105 | * **tau** Float. The time constant for first order actuator dynamics. 106 | * **dot_max** Float. Saturation on the magnitude of the derivative of the state for second order actuator dynamics. 107 | * **disabled** Boolean. Sets the state to disabled and its value to zero. E.g. if the aircraft in question has no rudder, 108 | and rudder is not virtualized through any other actuators. 109 | 110 | ### Actuation 111 | The actuation block defines the inputs given to the simulator, and the states used to simulate the actuator dynamics. 112 | Note that these can be different from the actuators used in the model (i.e. elevator, aileron, rudder, throttle), e.g. 113 | if the aircraft in question has elevons instead of elevator and aileron, in which case the actuator states will be 114 | mapped to the ones required by the simulator model. 115 | 116 | * **dynamics** List of strings. The names of the states used to simulate the actuator dynamics of the aircraft. These should be the 117 | actuators physically present on the aircraft. 118 | * **inputs** List of strings. The names of the states for which setpoints are fed to the step() function. 119 | 120 | ### Plots 121 | Plots can easily be constructed through the plots block. Each entry gets its own plot in the figure produced by render 122 | when mode="plot". A plot can support any number of states, so long as the set of state contains a maximum of two unique units. 123 | States with differing unit from the first listed state will be plotted on a twin y-axis on the right side of the figure. 124 | * **title**: String. Title of the plot. 125 | * **x_unit**. String. The unit of the x-axis. One of "seconds" or "timesteps". 126 | * **states**. List of strings. The names of the states to be included in the plot. 127 | * **plot_quantity**. String. Only used for actuator states. What quantity of the state to plot, one of "value", "dot" 128 | or "command". 129 | 130 | Additionally, each state can be plotted by itself by calling its plot function. 131 | 132 | ## Citation 133 | If you use this software, please cite: 134 | 135 | ```text 136 | @inproceedings{bohn2019deep, 137 | title={Deep Reinforcement Learning Attitude Control of Fixed-Wing UAVs Using Proximal Policy optimization}, 138 | author={B{\o}hn, Eivind and Coates, Erlend M and Moe, Signe and Johansen, Tor Arne}, 139 | booktitle={2019 International Conference on Unmanned Aircraft Systems (ICUAS)}, 140 | pages={523--533}, 141 | year={2019}, 142 | organization={IEEE} 143 | } 144 | ``` 145 | 146 | ## Changelog 147 | 148 | ### Release 0.1.2 (2020-04-23) 149 | 150 | --- 151 | 152 | * Reworked Dryden Turbulence Model. 153 | * Refactored code, adding docstrings to functions. 154 | * Changed method for specifying white noise input to shaping filter to allow for deterministic outputs. 155 | * Fix bug where filters were simulated for more steps than intended. 156 | * Fix bug where wrong white noise input were provided to the omega_q and omega_r shaping filters. 157 | * Added parameter for turbulence simulation length in configuration file. 158 | 159 | * Updated README. 160 | 161 | * PyFly is now available on PyPI under the name [pyfly-fixed-wing](https://pypi.org/project/pyfly-fixed-wing/). 162 | 163 | ### Release 0.1.1 (2019-08-20) 164 | 165 | --- 166 | 167 | * Reworked actuation module 168 | * Added actuation block to config file, specifying the input states to the step function and the states used 169 | to simulate the actuator dynamics. 170 | * PyFly now supports directly inputing elevon commands 171 | 172 | * Added optional energy states. When present in the states block of the config file, PyFly will calculate and record 173 | the specified energy states, allowing them to be inspected and plotted. 174 | 175 | * Target bounds can now be specified for variable plot. When the state value is within the bound of the target value, 176 | the bound area will be shaded with the lines' color. 177 | 178 | * Updated README and example. 179 | 180 | 181 | 182 | 183 | 184 | -------------------------------------------------------------------------------- /examples/cd_beta.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/eivindeb/pyfly/8451687b8df4a7bc6f78f3a1cdcfd503b76bf596/examples/cd_beta.png -------------------------------------------------------------------------------- /examples/coefficients.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/eivindeb/pyfly/8451687b8df4a7bc6f78f3a1cdcfd503b76bf596/examples/coefficients.png -------------------------------------------------------------------------------- /examples/render.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/eivindeb/pyfly/8451687b8df4a7bc6f78f3a1cdcfd503b76bf596/examples/render.png -------------------------------------------------------------------------------- /pyfly/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/eivindeb/pyfly/8451687b8df4a7bc6f78f3a1cdcfd503b76bf596/pyfly/__init__.py -------------------------------------------------------------------------------- /pyfly/dryden.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.signal import lti 3 | import math 4 | 5 | 6 | class Filter: 7 | def __init__(self, num, den): 8 | """ 9 | Wrapper for the scipy LTI system class. 10 | :param num: numerator of transfer function 11 | :param den: denominator of transfer function 12 | """ 13 | self.filter = lti(num, den) 14 | self.x = None 15 | self.y = None 16 | self.t = None 17 | 18 | def simulate(self, u, t): 19 | """ 20 | Simulate filter 21 | :param u: filter input 22 | :param t: time steps for which to simulate 23 | :return: filter output 24 | """ 25 | if self.x is None: 26 | x_0 = None 27 | else: 28 | x_0 = self.x[-1] 29 | 30 | self.t, self.y, self.x = self.filter.output(U=u, T=t, X0=x_0) 31 | 32 | return self.y 33 | 34 | def reset(self): 35 | """ 36 | Reset filter 37 | :return: 38 | """ 39 | self.x = None 40 | self.y = None 41 | self.t = None 42 | 43 | 44 | class DrydenGustModel: 45 | def __init__(self, dt, b, h=100, V_a=25, intensity=None): 46 | """ 47 | Python realization of the continuous Dryden Turbulence Model (MIL-F-8785C). 48 | :param dt: (float) band-limited white noise input sampling time. 49 | :param b: (float) wingspan of aircraft 50 | :param h: (float) Altitude of aircraft 51 | :param V_a: (float) Airspeed of aircraft 52 | :param intensity: (str) Intensity of turbulence, one of ["light", "moderate", "severe"] 53 | """ 54 | # For fixed (nominal) altitude and airspeed 55 | h = h # altitude [m] 56 | V_a = V_a # airspeed [m/s] 57 | 58 | # Conversion factors 59 | # 1 meter = 3.281 feet 60 | meters2feet = 3.281 61 | feet2meters = 1 / meters2feet 62 | # 1 knot = 0.5144 m/s 63 | knots2mpers = 0.5144 64 | 65 | if intensity is None: 66 | W_20 = 15 * knots2mpers # light turbulence 67 | elif intensity == "light": 68 | W_20 = 15 * knots2mpers # light turbulence 69 | elif intensity == "moderate": 70 | W_20 = 30 * knots2mpers # moderate turbulence 71 | elif intensity == "severe": 72 | W_20 = 45 * knots2mpers # severe turbulence 73 | else: 74 | raise Exception("Unsupported intensity type") 75 | 76 | # Convert meters to feet and follow MIL-F-8785C spec 77 | h = h * meters2feet 78 | b = b * meters2feet 79 | V_a = V_a * meters2feet 80 | W_20 = W_20 * meters2feet 81 | 82 | # Turbulence intensities 83 | sigma_w = 0.1 * W_20 84 | sigma_u = sigma_w / (0.177 + 0.000823 * h) ** 0.4 85 | sigma_v = sigma_u 86 | 87 | # Turbulence length scales 88 | L_u = h / (0.177 + 0.000823 * h) ** 1.2 89 | L_v = L_u 90 | L_w = h 91 | 92 | K_u = sigma_u * math.sqrt((2 * L_u) / (math.pi * V_a)) 93 | K_v = sigma_v * math.sqrt((L_v) / (math.pi * V_a)) 94 | K_w = sigma_w * math.sqrt((L_w) / (math.pi * V_a)) 95 | 96 | T_u = L_u / V_a 97 | T_v1 = math.sqrt(3.0) * L_v / V_a 98 | T_v2 = L_v / V_a 99 | T_w1 = math.sqrt(3.0) * L_w / V_a 100 | T_w2 = L_w / V_a 101 | 102 | K_p = sigma_w * math.sqrt(0.8 / V_a) * ((math.pi / (4 * b)) ** (1 / 6)) / ((L_w) ** (1 / 3)) 103 | K_q = 1 / V_a 104 | K_r = K_q 105 | 106 | T_p = 4 * b / (math.pi * V_a) 107 | T_q = T_p 108 | T_r = 3 * b / (math.pi * V_a) 109 | 110 | self.filters = {"H_u": Filter(feet2meters * K_u, [T_u, 1]), 111 | "H_v": Filter([feet2meters * K_v * T_v1, feet2meters * K_v], [T_v2 ** 2, 2 * T_v2, 1]), 112 | "H_w": Filter([feet2meters * K_w * T_w1, feet2meters * K_w], [T_w2 ** 2, 2 * T_w2, 1]), 113 | "H_p": Filter(K_p, [T_p, 1]), 114 | "H_q": Filter([-K_w * K_q * T_w1, -K_w * K_q, 0], [T_q * T_w2 ** 2, T_w2 ** 2 + 2 * T_q * T_w2, T_q + 2 * T_w2, 1]), 115 | "H_r": Filter([K_v * K_r * T_v1, K_v * K_r, 0], [T_r * T_v2 ** 2, T_v2 ** 2 + 2 * T_r * T_v2, T_r + 2 * T_v2, 1]),} 116 | 117 | self.np_random = None 118 | self.seed() 119 | 120 | self.dt = dt 121 | self.sim_length = None 122 | 123 | self.noise = None 124 | 125 | self.vel_lin = None 126 | self.vel_ang = None 127 | 128 | def seed(self, seed=None): 129 | """ 130 | Seed the random number generator. 131 | :param seed: (int) seed. 132 | :return: 133 | """ 134 | self.np_random = np.random.RandomState(seed) 135 | 136 | def _generate_noise(self, size): 137 | return np.sqrt(np.pi / self.dt) * self.np_random.standard_normal(size=(4, size)) 138 | 139 | def reset(self, noise=None): 140 | """ 141 | Reset model. 142 | :param noise: (np.array) Input to filters, should be four sequences of Gaussianly distributed numbers. 143 | :return: 144 | """ 145 | self.vel_lin = None 146 | self.vel_ang = None 147 | self.sim_length = 0 148 | 149 | if noise is not None: 150 | assert len(noise.shape) == 2 151 | assert noise.shape[0] == 4 152 | noise = noise * math.sqrt(math.pi / self.dt) 153 | self.noise = noise 154 | 155 | for filter in self.filters.values(): 156 | filter.reset() 157 | 158 | def simulate(self, length): 159 | """ 160 | Simulate turbulence by passing band-limited Gaussian noise of length length through the shaping filters. 161 | :param length: (int) the number of steps to simulate. 162 | :return: 163 | """ 164 | t_span = [self.sim_length, self.sim_length + length] 165 | 166 | t = np.linspace(t_span[0] * self.dt, t_span[1] * self.dt, length) 167 | 168 | if self.noise is None: 169 | noise = self._generate_noise(t.shape[0]) 170 | else: 171 | if self.noise.shape[-1] >= t_span[1]: 172 | noise = self.noise[:, t_span[0]:t_span[1]] 173 | else: 174 | noise_start_i = t_span[0] % self.noise.shape[-1] 175 | remaining_noise_length = self.noise.shape[-1] - noise_start_i 176 | if remaining_noise_length >= length: 177 | noise = self.noise[:, noise_start_i:noise_start_i + length] 178 | else: 179 | if length - remaining_noise_length > self.noise.shape[-1]: 180 | concat_noise = np.pad(self.noise, 181 | ((0, 0), (0, length - remaining_noise_length - self.noise.shape[-1])), 182 | mode="wrap") 183 | else: 184 | concat_noise = self.noise[:, :length - remaining_noise_length] 185 | noise = np.concatenate((self.noise[:, noise_start_i:], concat_noise), axis=-1) 186 | 187 | vel_lin = np.array([self.filters["H_u"].simulate(noise[0], t), 188 | self.filters["H_v"].simulate(noise[1], t), 189 | self.filters["H_w"].simulate(noise[2], t)]) 190 | 191 | vel_ang = np.array([self.filters["H_p"].simulate(noise[3], t), 192 | self.filters["H_q"].simulate(noise[1], t), 193 | self.filters["H_r"].simulate(noise[2], t)]) 194 | 195 | if self.vel_lin is None: 196 | self.vel_lin = vel_lin 197 | self.vel_ang = vel_ang 198 | else: 199 | self.vel_lin = np.concatenate((self.vel_lin, vel_lin), axis=1) 200 | self.vel_ang = np.concatenate((self.vel_ang, vel_ang), axis=1) 201 | 202 | self.sim_length += length -------------------------------------------------------------------------------- /pyfly/pid_controller.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class PIDController: 5 | def __init__(self, dt=0.01): 6 | 7 | self.k_p_V = 0.5 8 | self.k_i_V = 0.1 9 | self.k_p_phi = 1 10 | self.k_i_phi = 0 # Note gain is set to zero for roll channel 11 | self.k_d_phi = 0.5 12 | self.k_p_theta = -4 13 | self.k_i_theta = -0.75 14 | self.k_d_theta = -0.1 15 | 16 | self.delta_a_min = np.radians(-30) 17 | self.delta_e_min = np.radians(-30) 18 | self.delta_a_max = np.radians(30) 19 | self.delta_e_max = np.radians(35) 20 | 21 | self.dt = dt 22 | 23 | self.va_r = None 24 | self.phi_r = None 25 | self.theta_r = None 26 | 27 | self.int_va = 0 28 | self.int_roll = 0 29 | self.int_pitch = 0 30 | 31 | def set_reference(self, phi, theta, va): 32 | self.va_r = va 33 | self.phi_r = phi 34 | self.theta_r = theta 35 | 36 | def reset(self): 37 | self.int_va = 0 38 | self.int_roll = 0 39 | self.int_pitch = 0 40 | 41 | def get_action(self, phi, theta, va, omega): 42 | e_V_a = va - self.va_r 43 | e_phi = phi - self.phi_r 44 | e_theta = theta - self.theta_r 45 | 46 | # (integral states are initialized to zero) 47 | self.int_va = self.int_va + self.dt * e_V_a 48 | self.int_roll = self.int_roll + self.dt * e_phi 49 | self.int_pitch = self.int_pitch + self.dt * e_theta 50 | 51 | # Note the different sign on pitch gains below. 52 | # Positive aileron -> positive roll moment 53 | # Positive elevator -> NEGATIVE pitch moment 54 | 55 | delta_t = 0 - self.k_p_V * e_V_a - self.k_i_V * self.int_va # PI 56 | delta_a = - self.k_p_phi * e_phi - self.k_i_phi * self.int_roll - self.k_d_phi * omega[0] # PID 57 | delta_e = 0 - self.k_p_theta * e_theta - self.k_i_theta * self.int_pitch - self.k_d_theta * omega[1] # PID 58 | delta_r = 0 # No rudder available 59 | 60 | # Constrain input 61 | delta_t = np.clip(delta_t, 0, 1.0) # throttle between 0 and 1 62 | 63 | delta_a = np.clip(delta_a, self.delta_a_min, self.delta_a_max) 64 | delta_e = np.clip(delta_e, self.delta_e_min, self.delta_e_max) 65 | 66 | return np.asarray([delta_e, delta_a, delta_t]) 67 | 68 | -------------------------------------------------------------------------------- /pyfly/pyfly.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import scipy.integrate 4 | import scipy.io 5 | import os.path as osp 6 | import json 7 | import matplotlib.pyplot as plt 8 | import matplotlib.gridspec 9 | 10 | 11 | class ConstraintException(Exception): 12 | def __init__(self, variable, value, limit): 13 | self.message = "Constraint on {} violated ({}/{})".format(variable, value, limit) 14 | self.variable = variable 15 | 16 | 17 | class Variable: 18 | def __init__(self, name, value_min=None, value_max=None, init_min=None, init_max=None, constraint_min=None, 19 | constraint_max=None, convert_to_radians=False, unit=None, label=None, wrap=False): 20 | """ 21 | PyFly state object managing state history, constraints and visualizations. 22 | 23 | :param name: (string) name of state 24 | :param value_min: (float) lowest possible value of state, values will be clipped to this limit 25 | :param value_max: (float) highest possible value of state, values will be clipped to this limit 26 | :param init_min: (float) lowest possible initial value of state 27 | :param init_max: (float) highest possible initial value of state 28 | :param constraint_min: (float) lower constraint of state, which if violated will raise ConstraintException 29 | :param constraint_max: (float) upper constraint of state, which if violated will raise ConstraintException 30 | :param convert_to_radians: (bool) whether to convert values for attributes from configuration file from degrees 31 | to radians 32 | :param unit: (string) unit of the state, for plotting purposes 33 | :param label: (string) label given to state in plots 34 | :param wrap: (bool) whether to wrap state value in region [-pi, pi] 35 | """ 36 | self.value_min = value_min 37 | self.value_max = value_max 38 | 39 | self.init_min = init_min if init_min is not None else value_min 40 | self.init_max = init_max if init_max is not None else value_max 41 | 42 | self.constraint_min = constraint_min 43 | self.constraint_max = constraint_max 44 | 45 | if convert_to_radians: 46 | for attr_name, val in self.__dict__.items(): 47 | if val is not None: 48 | setattr(self, attr_name, np.radians(val)) 49 | 50 | self.name = name 51 | 52 | self.value = None 53 | 54 | self.wrap = wrap 55 | 56 | self.unit = unit 57 | self.label = label if label is not None else self.name 58 | self.lines = {"self": None} 59 | self.target_lines = {"self": None} 60 | self.target_bounds = {"self": None} 61 | 62 | self.np_random = None 63 | self.seed() 64 | 65 | self.history = None 66 | 67 | def reset(self, value=None): 68 | """ 69 | Reset object to initial state. 70 | 71 | :param value: (float) initial value of state 72 | """ 73 | self.history = [] 74 | 75 | if value is None: 76 | try: 77 | value = self.np_random.uniform(self.init_min, self.init_max) 78 | except TypeError: 79 | raise Exception("Variable init_min and init_max can not be None if no value is provided on reset") 80 | else: 81 | value = self.apply_conditions(value) 82 | 83 | self.value = value 84 | 85 | self.history.append(value) 86 | 87 | def seed(self, seed=None): 88 | """ 89 | Seed random number generator of state 90 | 91 | :param seed: (int) seed of random state 92 | """ 93 | self.np_random = np.random.RandomState(seed) 94 | 95 | def apply_conditions(self, value): 96 | """ 97 | Apply state limits and constraints to value. Will raise ConstraintException if constraints are violated 98 | 99 | :param value: (float) value to which limits and constraints are applied 100 | :return: (float) value after applying limits and constraints 101 | """ 102 | if self.constraint_min is not None and value < self.constraint_min: 103 | raise ConstraintException(self.name, value, self.constraint_min) 104 | 105 | if self.constraint_max is not None and value > self.constraint_max: 106 | raise ConstraintException(self.name, value, self.constraint_max) 107 | 108 | if self.value_min is not None or self.value_max is not None: 109 | value = np.clip(value, self.value_min, self.value_max) 110 | 111 | if self.wrap and np.abs(value) > np.pi: 112 | value = np.sign(value) * (np.abs(value) % np.pi - np.pi) 113 | 114 | return value 115 | 116 | def set_value(self, value, save=True): 117 | """ 118 | Set value of state, after applying limits and constraints to value. Raises ConstraintException if constraints 119 | are violated 120 | 121 | :param value: (float) new value of state 122 | :param save: (bool) whether to commit value to history of state 123 | """ 124 | value = self.apply_conditions(value) 125 | 126 | if save: 127 | self.history.append(value) 128 | 129 | self.value = value 130 | 131 | def plot(self, axis=None, y_unit=None, target=None, plot_id=None, **plot_kw): 132 | """ 133 | Plot state history. 134 | 135 | :param axis: (matplotlib.pyplot.axis or None) axis object to plot state to. If None create new axis 136 | :param y_unit: (string) unit state should be plotted in, will convert values if different from internal 137 | representation 138 | :param target: (list) target values for state, must be of equal size to state history 139 | :param plot_id: (string or int or None) identifier of parent plot object. Allows state to plot to multiple 140 | figures at a time. 141 | :param plot_kw: (dict) plot keyword arguments passed to matplotlib.pyplot.plot 142 | """ 143 | 144 | def linear_scaling(val, old_min, old_max, new_min, new_max): 145 | return (new_max - np.sign(old_min) * (- new_min)) / (old_max - old_min) * ( 146 | np.array(val) - old_max) + new_max 147 | 148 | if y_unit is None: 149 | y_unit = self.unit if y_unit is None else y_unit 150 | 151 | x, y = self._get_plot_x_y_data() 152 | if "degrees" in y_unit: 153 | y = np.degrees(y) 154 | if target is not None: 155 | target["data"] = np.degrees(target["data"]) 156 | if "bound" in target: 157 | target["bound"] = np.degrees(target["bound"]) 158 | elif y_unit == "%": # TODO: scale positive according to positive limit and negative according to lowest minimum value 159 | y = linear_scaling(y, self.value_min, self.value_max, -100, 100) 160 | if target is not None: 161 | target["data"] = linear_scaling(target["data"], self.value_min, self.value_max, -100, 100) 162 | if "bound" in target: 163 | target["bound"] = linear_scaling(target["bound"], self.value_min, self.value_max, -100, 100) 164 | else: 165 | y = y 166 | 167 | plot_object = axis 168 | if axis is None: 169 | plot_object = plt 170 | plot_id = "self" 171 | fig_kw = {"title": self.name, "ylabel": y_unit} 172 | 173 | if self.lines.get(plot_id, None) is None: 174 | line, = plot_object.plot(x, y, label=self.label, **plot_kw) 175 | self.lines[plot_id] = line 176 | 177 | if target is not None: 178 | tar_line, = plot_object.plot(x, target["data"], color=self.lines[plot_id].get_color(), linestyle="dashed", 179 | marker="x", markevery=0.2) 180 | 181 | if "bound" in target: 182 | tar_bound = plot_object.fill_between(np.arange(target["bound"].shape[0]), 183 | target["data"] + target["bound"], 184 | target["data"] - target["bound"], alpha=0.15, 185 | facecolor=self.lines[plot_id].get_color() 186 | ) 187 | self.target_bounds[plot_id] = tar_bound 188 | self.target_lines[plot_id] = tar_line 189 | else: 190 | self.lines[plot_id].set_data(x, y) 191 | if target is not None: 192 | self.target_lines[plot_id].set_data(x, target) 193 | if "bound" in target: # TODO: fix this? 194 | self.target_bounds[plot_id].set_data(np.arange(target["bound"].shape[0]), 195 | target["data"] + target["bound"], 196 | target["data"] - target["bound"]) 197 | if axis is None: 198 | for k, v in fig_kw.items(): 199 | getattr(plot_object, format(k))(v) 200 | plt.show() 201 | 202 | def close_plot(self, plot_id="self"): 203 | """ 204 | Close plot with id plot_id. 205 | 206 | :param plot_id: (string or int) identifier of parent plot object 207 | """ 208 | self.lines[plot_id] = None 209 | self.target_lines[plot_id] = None 210 | self.target_bounds[plot_id] = None 211 | 212 | def _get_plot_x_y_data(self): 213 | """ 214 | Get plot data from variable history. 215 | 216 | :return: ([int], [float]) x plot data, y plot data 217 | """ 218 | x = list(range(len(self.history))) 219 | y = self.history 220 | return x, y 221 | 222 | 223 | class ControlVariable(Variable): 224 | def __init__(self, order=None, tau=None, omega_0=None, zeta=None, dot_max=None, disabled=False, **kwargs): 225 | """ 226 | PyFly actuator state variable. 227 | 228 | :param order: (int) order of state transfer function 229 | :param tau: (float) time constant for first order transfer functions 230 | :param omega_0: (float) undamped natural frequency of second order transfer functions 231 | :param zeta: (float) damping factor of second order transfer function 232 | :param dot_max: (float) constraint on magnitude of derivative of second order transfer function 233 | :param disabled: (bool) if actuator is disabled for aircraft, e.g. aircraft has no rudder 234 | :param kwargs: (dict) keyword arguments for Variable class 235 | """ 236 | assert (disabled or (order == 1 or order == 2)) 237 | super().__init__(**kwargs) 238 | self.order = order 239 | self.tau = tau 240 | self.omega_0 = omega_0 241 | self.zeta = zeta 242 | self.dot_max = dot_max 243 | 244 | if order == 1: 245 | assert (tau is not None) 246 | self.coefs = [[-1 / self.tau, 0, 1 / self.tau], [0, 0, 0]] 247 | elif order == 2: 248 | assert (omega_0 is not None and zeta is not None) 249 | self.coefs = [[0, 1, 0], [-self.omega_0 ** 2, -2 * self.zeta * self.omega_0, self.omega_0 ** 2]] 250 | self.dot = None 251 | self.command = None 252 | self.disabled = disabled 253 | if self.disabled: 254 | self.value = 0 255 | self.plot_quantity = "value" 256 | 257 | def apply_conditions(self, values): 258 | """ 259 | Apply state limits and constraints to value. Will raise ConstraintException if constraints are violated 260 | 261 | :param value: (float) value to which limits and constraints is applied 262 | :return: (float) value after applying limits and constraints 263 | """ 264 | try: 265 | value, dot = values 266 | except: 267 | value, dot = values, 0 268 | value = super().apply_conditions(value) 269 | 270 | if self.dot_max is not None: 271 | dot = np.clip(dot, -self.dot_max, self.dot_max) 272 | 273 | return [value, dot] 274 | 275 | def set_command(self, command): 276 | """ 277 | Set setpoint for actuator and commit to history of state 278 | 279 | :param command: setpoint for actuator 280 | """ 281 | command = super().apply_conditions(command) 282 | self.command = command 283 | self.history["command"].append(command) 284 | 285 | def reset(self, value=None): 286 | """ 287 | Reset object to initial state. 288 | 289 | :param value: (list) initial value, derivative and setpoint of state 290 | """ 291 | self.history = {"value": [], "dot": [], "command": []} 292 | 293 | if not self.disabled: 294 | if value is None: 295 | value = self.np_random.uniform(self.init_min, self.init_max), 0 296 | else: 297 | value = self.apply_conditions(value) 298 | 299 | self.value = value[0] 300 | self.dot = value[1] 301 | command = None 302 | self.command = command 303 | else: 304 | value, dot, command = 0, 0, None 305 | self.value = value 306 | self.dot = dot 307 | self.command = command 308 | 309 | self.history["value"].append(self.value) 310 | self.history["dot"].append(self.dot) 311 | 312 | def set_value(self, value, save=True): 313 | """ 314 | Set value of state, after applying limits and constraints to value. Raises ConstraintException if constraints 315 | are violated 316 | 317 | :param value: (float) new value and derivative of state 318 | :param save: (bool) whether to commit value to history of state 319 | """ 320 | value, dot = self.apply_conditions(value) 321 | 322 | self.value = value 323 | self.dot = dot 324 | 325 | if save: 326 | self.history["value"].append(value) 327 | self.history["dot"].append(dot) 328 | 329 | def _get_plot_x_y_data(self): 330 | """ 331 | Get plot data from variable history, for the quantity designated by the attribute plot_quantity. 332 | 333 | :return: ([int], [float]) x plot data, y plot data 334 | """ 335 | y = self.history[self.plot_quantity] 336 | x = list(range(len(y))) 337 | return x, y 338 | 339 | def get_coeffs(self): 340 | if self.order == 1: 341 | return 342 | else: 343 | return [] 344 | 345 | 346 | class EnergyVariable(Variable): 347 | def __init__(self, mass=None, inertia_matrix=None, gravity=None, **kwargs): 348 | super().__init__(**kwargs) 349 | self.required_variables = [] 350 | self.variables = {} 351 | if self.name == "energy_potential" or self.name == "energy_total": 352 | assert(mass is not None and gravity is not None) 353 | self.mass = mass 354 | self.gravity = gravity 355 | self.required_variables.append("position_d") 356 | if self.name == "energy_kinetic" or self.name == "energy_total": 357 | assert (mass is not None and inertia_matrix is not None) 358 | self.mass = mass 359 | self.inertia_matrix = inertia_matrix 360 | self.required_variables.extend(["Va", "omega_p", "omega_q", "omega_r"]) 361 | if self.name == "energy_kinetic_rotational": 362 | assert(inertia_matrix is not None) 363 | self.inertia_matrix = inertia_matrix 364 | self.required_variables.extend(["omega_p", "omega_q", "omega_r"]) 365 | if self.name == "energy_kinetic_translational": 366 | assert(mass is not None) 367 | self.mass = mass 368 | self.required_variables.append("Va") 369 | 370 | def add_requirement(self, name, variable): 371 | self.variables[name] = variable 372 | 373 | def calculate_value(self): 374 | val = 0 375 | if self.name == "energy_potential" or self.name == "energy_total": 376 | val += self.mass * self.gravity * (-self.variables["position_d"].value) 377 | if self.name == "energy_kinetic_rotational" or self.name == "energy_kinetic" or self.name == "energy_total": 378 | for i, axis in enumerate(["omega_p", "omega_q", "omega_r"]): 379 | m_i = self.inertia_matrix[i, i] 380 | val += 1 / 2 * m_i * self.variables[axis].value ** 2 381 | if self.name == "energy_kinetic_translational" or self.name == "energy_kinetic" or self.name == "energy_total": 382 | val += 1 / 2 * self.mass * self.variables["Va"].value ** 2 383 | 384 | return val 385 | 386 | 387 | class Actuation: 388 | def __init__(self, model_inputs, actuator_inputs, dynamics): 389 | """ 390 | PyFly actuation object, responsible for verifying validity of configured actuator model, processing inputs and 391 | actuator dynamics. 392 | 393 | :param model_inputs: ([string]) the states used by PyFly as inputs to dynamics 394 | :param actuator_inputs: ([string]) the user configured actuator input states 395 | :param dynamics: ([string]) the user configured actuator states to simulate dynamics for 396 | """ 397 | self.states = {} 398 | self.coefficients = [[np.array([]) for _ in range(3)] for __ in range(2)] 399 | self.elevon_dynamics = False 400 | self.dynamics = dynamics 401 | self.inputs = actuator_inputs 402 | self.model_inputs = model_inputs 403 | self.input_indices = {s: i for i, s in enumerate(actuator_inputs)} 404 | self.dynamics_indices = {s: i for i, s in enumerate(dynamics)} 405 | 406 | def set_states(self, values, save=True): 407 | """ 408 | Set values of actuator states. 409 | 410 | :param values: ([float]) list of state values + list of state derivatives 411 | :param save: (bool) whether to commit values to state history 412 | :return: 413 | """ 414 | for i, state in enumerate(self.dynamics): 415 | self.states[state].set_value((values[i], values[len(self.dynamics) + i]), save=save) 416 | 417 | # Simulator model operates on elevator and aileron angles, if aircraft has elevon dynamics need to map 418 | if self.elevon_dynamics: 419 | elevator, aileron = self._map_elevon_to_elevail(er=self.states["elevon_right"].value, 420 | el=self.states["elevon_left"].value) 421 | 422 | self.states["aileron"].set_value((aileron, 0), save=save) 423 | self.states["elevator"].set_value((elevator, 0), save=save) 424 | 425 | def add_state(self, state): 426 | """ 427 | Add actuator state, and configure dynamics if state has dynamics. 428 | 429 | :param state: (ControlVariable) actuator state 430 | :return: 431 | """ 432 | self.states[state.name] = state 433 | if state.name in self.dynamics: 434 | for i in range(2): 435 | for j in range(3): 436 | self.coefficients[i][j] = np.append(self.coefficients[i][j], state.coefs[i][j]) 437 | 438 | def get_values(self): 439 | """ 440 | Get state values and derivatives for states in actuator dynamics. 441 | 442 | :return: ([float]) list of state values + list of state derivatives 443 | """ 444 | return [self.states[state].value for state in self.dynamics] + [self.states[state].dot for state in 445 | self.dynamics] 446 | 447 | def rhs(self, setpoints=None): 448 | """ 449 | Right hand side of actuator differential equation. 450 | 451 | :param setpoints: ([float] or None) setpoints for actuators. If None, setpoints are set as the current command 452 | of the dynamics variable 453 | :return: ([float]) right hand side of actuator differential equation. 454 | """ 455 | if setpoints is None: 456 | setpoints = [self.states[state].command for state in self.dynamics] 457 | states = [self.states[state].value for state in self.dynamics] 458 | dots = [self.states[state].dot for state in self.dynamics] 459 | dot = np.multiply(states, 460 | self.coefficients[0][0]) + np.multiply(setpoints, 461 | self.coefficients[0][2]) + np.multiply(dots, self.coefficients[0][1]) 462 | ddot = np.multiply(states, 463 | self.coefficients[1][0]) + np.multiply(setpoints, 464 | self.coefficients[1][2]) + np.multiply(dots, self.coefficients[1][1]) 465 | 466 | return np.concatenate((dot, ddot)) 467 | 468 | def set_and_constrain_commands(self, commands): 469 | """ 470 | Take raw actuator commands and constrain them according to the state limits and constraints, and update state 471 | values and history. 472 | 473 | :param commands: ([float]) raw commands 474 | :return: ([float]) constrained commands 475 | """ 476 | dynamics_commands = {} 477 | if self.elevon_dynamics and "elevator" and "aileron" in self.inputs: 478 | elev_c, ail_c = commands[self.input_indices["elevator"]], commands[self.input_indices["aileron"]] 479 | elevon_r_c, elevon_l_c = self._map_elevail_to_elevon(elev=elev_c, ail=ail_c) 480 | dynamics_commands = {"elevon_right": elevon_r_c, "elevon_left": elevon_l_c} 481 | 482 | for state in self.dynamics: 483 | if state in self.input_indices: 484 | state_c = commands[self.input_indices[state]] 485 | else: # Elevail inputs with elevon dynamics 486 | state_c = dynamics_commands[state] 487 | self.states[state].set_command(state_c) 488 | dynamics_commands[state] = self.states[state].command 489 | 490 | # The elevator and aileron commands constrained by limitatons on physical elevons 491 | if self.elevon_dynamics: 492 | elev_c, ail_c = self._map_elevon_to_elevail(er=dynamics_commands["elevon_right"], 493 | el=dynamics_commands["elevon_left"]) 494 | self.states["elevator"].set_command(elev_c) 495 | self.states["aileron"].set_command(ail_c) 496 | 497 | for state, i in self.input_indices.items(): 498 | commands[i] = self.states[state].command 499 | 500 | return commands 501 | 502 | def finalize(self): 503 | """ 504 | Assert valid configuration of actuator dynamics and set actuator state limits if applicable. 505 | """ 506 | if "elevon_left" in self.dynamics or "elevon_right" in self.dynamics: 507 | assert("elevon_left" in self.dynamics and "elevon_right" in self.dynamics and not ("aileron" in self.dynamics 508 | or "elevator" in self.dynamics)) 509 | assert ("elevon_left" in self.states and "elevon_right" in self.states) 510 | self.elevon_dynamics = True 511 | 512 | # Set elevator and aileron limits from elevon limits for plotting purposes etc. 513 | if "elevator" in self.states: 514 | elev_min, _ = self._map_elevon_to_elevail(er=self.states["elevon_right"].value_min, 515 | el=self.states["elevon_left"].value_min) 516 | elev_max, _ = self._map_elevon_to_elevail(er=self.states["elevon_right"].value_max, 517 | el=self.states["elevon_left"].value_max) 518 | self.states["elevator"].value_min = elev_min 519 | self.states["elevator"].value_max = elev_max 520 | if "aileron" in self.states: 521 | _, ail_min = self._map_elevon_to_elevail(er=self.states["elevon_right"].value_max, 522 | el=self.states["elevon_left"].value_min) 523 | _, ail_max = self._map_elevon_to_elevail(er=self.states["elevon_right"].value_min, 524 | el=self.states["elevon_left"].value_max) 525 | self.states["aileron"].value_min = ail_min 526 | self.states["aileron"].value_max = ail_max 527 | 528 | def reset(self, state_init=None): 529 | for state in self.dynamics: 530 | init = None 531 | if state_init is not None and state in state_init: 532 | init = state_init[state] 533 | self.states[state].reset(value=init) 534 | 535 | if self.elevon_dynamics: 536 | elev, ail = self._map_elevon_to_elevail(er=self.states["elevon_right"].value, el=self.states["elevon_left"].value) 537 | self.states["elevator"].reset(value=elev) 538 | self.states["aileron"].reset(value=ail) 539 | 540 | def _map_elevail_to_elevon(self, elev, ail): 541 | er = -1 * ail + elev 542 | el = ail + elev 543 | return er, el 544 | 545 | def _map_elevon_to_elevail(self, er, el): 546 | ail = (-er + el) / 2 547 | elev = (er + el) / 2 548 | return elev, ail 549 | 550 | 551 | class AttitudeQuaternion: 552 | def __init__(self): 553 | """ 554 | Quaternion attitude representation used by PyFly. 555 | """ 556 | self.quaternion = None 557 | self.euler_angles = {"roll": None, "pitch": None, "yaw": None} 558 | self.history = None 559 | 560 | def seed(self, seed): 561 | return 562 | 563 | def reset(self, euler_init): 564 | """ 565 | Reset state of attitude quaternion to value given by euler angles. 566 | 567 | :param euler_init: ([float]) the roll, pitch, yaw values to initialize quaternion to. 568 | """ 569 | if euler_init is not None: 570 | self._from_euler_angles(euler_init) 571 | else: 572 | raise NotImplementedError 573 | self.history = [self.quaternion] 574 | 575 | def as_euler_angle(self, angle="all", timestep=-1): 576 | """ 577 | Get attitude quaternion as euler angles, roll, pitch and yaw. 578 | 579 | :param angle: (string) which euler angle to return or all. 580 | :param timestep: (int) timestep 581 | :return: (float or dict) requested euler angles. 582 | """ 583 | e0, e1, e2, e3 = self.history[timestep] 584 | res = {} 585 | if angle == "roll" or angle == "all": 586 | res["roll"] = np.arctan2(2 * (e0 * e1 + e2 * e3), e0 ** 2 + e3 ** 2 - e1 ** 2 - e2 ** 2) 587 | if angle == "pitch" or angle == "all": 588 | res["pitch"] = np.arcsin(2 * (e0 * e2 - e1 * e3)) 589 | if angle == "yaw" or angle == "all": 590 | res["yaw"] = np.arctan2(2 * (e0 * e3 + e1 * e2), e0 ** 2 + e1 ** 2 - e2 ** 2 - e3 ** 2) 591 | 592 | return res if angle == "all" else res[angle] 593 | 594 | @property 595 | def value(self): 596 | return self.quaternion 597 | 598 | def _from_euler_angles(self, euler): 599 | """ 600 | Set value of attitude quaternion from euler angles. 601 | 602 | :param euler: ([float]) euler angles roll, pitch, yaw. 603 | """ 604 | phi, theta, psi = euler 605 | e0 = np.cos(psi / 2) * np.cos(theta / 2) * np.cos(phi / 2) + np.sin(psi / 2) * np.sin(theta / 2) * np.sin( 606 | phi / 2) 607 | e1 = np.cos(psi / 2) * np.cos(theta / 2) * np.sin(phi / 2) - np.sin(psi / 2) * np.sin(theta / 2) * np.cos( 608 | phi / 2) 609 | e2 = np.cos(psi / 2) * np.sin(theta / 2) * np.cos(phi / 2) + np.sin(psi / 2) * np.cos(theta / 2) * np.sin( 610 | phi / 2) 611 | e3 = np.sin(psi / 2) * np.cos(theta / 2) * np.cos(phi / 2) - np.cos(psi / 2) * np.sin(theta / 2) * np.sin( 612 | phi / 2) 613 | 614 | self.quaternion = (e0, e1, e2, e3) 615 | 616 | def set_value(self, quaternion, save=True): 617 | """ 618 | Set value of attitude quaternion. 619 | 620 | :param quaternion: ([float]) new quaternion value 621 | :param save: (bool) whether to commit value to history of attitude. 622 | """ 623 | self.quaternion = quaternion 624 | if save: 625 | self.history.append(self.quaternion) 626 | 627 | 628 | class Wind: 629 | def __init__(self, turbulence, mag_min=None, mag_max=None, b=None, turbulence_intensity=None, sim_length=300, dt=None): 630 | """ 631 | Wind and turbulence object used by PyFly. 632 | 633 | :param turbulence: (bool) whether turbulence is enabled 634 | :param mag_min: (float) minimum magnitude of steady wind component 635 | :param mag_max: (float) maximum magnitude of steady wind component 636 | :param b: (float) wingspan of aircraft 637 | :param turbulence_intensity: (string) intensity of turbulence 638 | :param dt: (float) integration step length 639 | """ 640 | self.turbulence = turbulence 641 | self.mag_min = mag_min 642 | self.mag_max = mag_max 643 | self.steady = None 644 | self.components = [] 645 | self.turbulence_sim_length = sim_length 646 | 647 | if self.turbulence: 648 | self.dryden = DrydenGustModel(self.turbulence_sim_length, dt, b, intensity=turbulence_intensity) 649 | else: 650 | self.dryden = None 651 | 652 | self.np_random = None 653 | self.seed() 654 | 655 | def seed(self, seed=None): 656 | """ 657 | Seed random number generator of object 658 | 659 | """ 660 | self.np_random = np.random.RandomState(seed) 661 | if self.turbulence: 662 | self.dryden.seed(seed) 663 | 664 | def reset(self, value=None, noise=None): 665 | """ 666 | Reset wind object to initial state 667 | 668 | :param value: ([float] or float) strength and direction of the n, e and d components or magnitude of the steady wind. 669 | """ 670 | if value is None or isinstance(value, float) or isinstance(value, int): 671 | if value is None and self.mag_min is None and self.mag_max is None: 672 | value = [] 673 | for comp in self.components: 674 | comp.reset() 675 | value.append(comp.value) 676 | else: 677 | if value is None: 678 | magnitude = self.np_random.uniform(self.mag_min, self.mag_max) 679 | else: 680 | magnitude = value 681 | w_n = self.np_random.uniform(-magnitude, magnitude) 682 | w_e_max = np.sqrt(magnitude ** 2 - w_n ** 2) 683 | w_e = self.np_random.uniform(-w_e_max, w_e_max) 684 | w_d = np.sqrt(magnitude ** 2 - w_n ** 2 - w_e ** 2) 685 | value = [w_n, w_e, w_d] 686 | 687 | if self.turbulence: 688 | self.dryden.reset(noise) 689 | 690 | self.steady = value 691 | for i, comp in enumerate(self.components): 692 | comp.reset(value[i]) 693 | 694 | def set_value(self, timestep): 695 | """ 696 | Set value to wind value at timestep t 697 | 698 | :param timestep: (int) timestep 699 | """ 700 | value = self.steady 701 | 702 | if self.turbulence: 703 | value += self._get_turbulence(timestep, "linear") 704 | 705 | for i, comp in enumerate(self.components): 706 | comp.set_value(value[i]) 707 | 708 | def get_turbulence_linear(self, timestep): 709 | """ 710 | Get linear component of turbulence model at given timestep 711 | 712 | :param timestep: (int) timestep 713 | :return: ([float]) linear component of turbulence at given timestep 714 | """ 715 | return self._get_turbulence(timestep, "linear") 716 | 717 | def get_turbulence_angular(self, timestep): 718 | """ 719 | Get angular component of turbulence model at given timestep 720 | 721 | :param timestep: (int) timestep 722 | :return: ([float]) angular component of turbulence at given timestep 723 | """ 724 | return self._get_turbulence(timestep, "angular") 725 | 726 | def _get_turbulence(self, timestep, component): 727 | """ 728 | Get turbulence at given timestep. 729 | 730 | :param timestep: (int) timestep 731 | :param component: (string) which component to return, linear or angular. 732 | :return: ([float]) turbulence component at timestep 733 | """ 734 | if timestep >= self.dryden.sim_length: 735 | self.dryden.simulate(self.turbulence_sim_length) 736 | 737 | if component == "linear": 738 | return self.dryden.vel_lin[:, timestep] 739 | else: 740 | return self.dryden.vel_ang[:, timestep] 741 | 742 | 743 | class Plot: 744 | def __init__(self, id, variables=None, title=None, x_unit=None, xlabel=None, ylabel=None, dt=None, 745 | plot_quantity=None): 746 | """ 747 | Plot object used by PyFly to house (sub)figures. 748 | 749 | :param id: (string or int) identifier of figure 750 | :param variables: ([string]) list of names of states included in figure 751 | :param title: (string) title of figure 752 | :param x_unit: (string) unit of x-axis, one of timesteps or seconds 753 | :param xlabel: (string) label for x-axis 754 | :param ylabel: (string) label for y-axis 755 | :param dt: (float) integration step length, required when x_unit is seconds. 756 | :param plot_quantity: (string) the attribute of the states that is plotted 757 | """ 758 | self.id = id 759 | self.title = title 760 | 761 | if x_unit is None: 762 | x_unit = "timesteps" 763 | elif x_unit not in ["timesteps", "seconds"]: 764 | raise Exception("Unsupported x unit (one of timesteps/seconds)") 765 | elif x_unit == "seconds" and dt is None: 766 | raise Exception("Parameter dt can not be none when x unit is seconds") 767 | 768 | self.x_unit = x_unit 769 | self.y_units = [] 770 | self.xlabel = xlabel 771 | self.ylabel = ylabel 772 | self.variables = variables 773 | self.axis = None 774 | self.dt = dt 775 | self.plot_quantity = plot_quantity 776 | 777 | def add_variable(self, var): 778 | """ 779 | Add state to plot 780 | 781 | :param var: (string) name of state 782 | """ 783 | if self.variables is None: 784 | self.variables = [] 785 | self.variables.append(var) 786 | 787 | if var.unit not in self.y_units: 788 | self.y_units.append(var.unit) 789 | 790 | if len(self.y_units) > 2: 791 | raise Exception("More than two different units in plot") 792 | 793 | def close(self): 794 | """ 795 | Close figure 796 | """ 797 | for var in self.variables: 798 | var.close_plot(self.id) 799 | 800 | self.axis = None 801 | 802 | def plot(self, fig=None, targets=None): 803 | """ 804 | Plot history of states in figure 805 | 806 | :param fig: (matplotlib.pyplot.figure) optional parent figure 807 | :param targets: (dict) target values for states in state name - values pairs 808 | """ 809 | first = False 810 | if self.axis is None: 811 | first = True 812 | 813 | if self.xlabel is not None: 814 | xlabel = self.xlabel 815 | else: 816 | if self.x_unit == "timesteps": 817 | xlabel = self.x_unit 818 | elif self.x_unit == "seconds": 819 | xlabel = "Time (s)" 820 | 821 | if self.ylabel is not None: 822 | ylabel = self.ylabel 823 | else: 824 | ylabel = self.y_units[0] 825 | 826 | if fig is not None: 827 | self.axis = {self.y_units[0]: plt.subplot(fig, title=self.title, xlabel=xlabel, ylabel=ylabel)} 828 | else: 829 | self.axis = {self.y_units[0]: plt.plot(title=self.title, xlabel=xlabel, ylabel=ylabel)} 830 | 831 | if len(self.y_units) > 1: 832 | self.axis[self.y_units[1]] = self.axis[self.y_units[0]].twinx() 833 | self.axis[self.y_units[1]].set_ylabel(self.y_units[1]) 834 | 835 | colors = plt.rcParams['axes.prop_cycle'].by_key()['color'] 836 | for i, v in enumerate(self.variables): 837 | if self.plot_quantity is not None: 838 | v.plot_quantity = self.plot_quantity 839 | target = targets[v.name] if targets is not None and v.name in targets else None 840 | v.plot(self.axis[v.unit], plot_id=self.id, target=target, color=colors[i]) 841 | 842 | if first: 843 | if len(self.y_units) > 1: 844 | labeled_lines = [] 845 | for ax in self.axis.values(): 846 | labeled_lines.extend([l for l in ax.lines if "_line" not in l.get_label()]) 847 | self.axis[self.y_units[0]].legend(labeled_lines, [l.get_label() for l in labeled_lines]) 848 | else: 849 | self.axis[self.y_units[0]].legend() 850 | 851 | else: 852 | for ax in self.axis.values(): 853 | ax.relim() 854 | ax.autoscale_view() 855 | 856 | if self.x_unit == "seconds": 857 | xticks = self.axis[self.y_units[0]].get_xticks() 858 | self.axis[self.y_units[0]].set_xticklabels(["{0:.1f}".format(tick * self.dt) for tick in xticks]) 859 | 860 | 861 | class PyFly: 862 | REQUIRED_VARIABLES = ["alpha", "beta", "roll", "pitch", "yaw", "omega_p", "omega_q", "omega_r", "position_n", 863 | "position_e", "position_d", "velocity_u", "velocity_v", "velocity_w", "Va", 864 | "elevator", "aileron", "rudder", "throttle"] 865 | 866 | def __init__(self, 867 | config_path=osp.join(osp.dirname(__file__), "pyfly_config.json"), 868 | parameter_path=osp.join(osp.dirname(__file__), "x8_param.mat"), 869 | config_kw=None): 870 | """ 871 | A flight simulator for fixed wing aircraft with configurable initial conditions, constraints and turbulence 872 | conditions 873 | 874 | :param config_path: (string) Path to json configuration file for simulator 875 | :param parameter_path: (string) Path to file containing required aircraft parameters 876 | """ 877 | 878 | def set_config_attrs(parent, kws): 879 | for attr, val in kws.items(): 880 | if isinstance(val, dict): 881 | set_config_attrs(parent[attr], val) 882 | else: 883 | parent[attr] = val 884 | 885 | _, parameter_extension = osp.splitext(parameter_path) 886 | if parameter_extension == ".mat": 887 | self.params = scipy.io.loadmat(parameter_path, squeeze_me=True) 888 | elif parameter_extension == ".json": 889 | with open(parameter_path) as param_file: 890 | self.params = json.load(param_file) 891 | else: 892 | raise Exception("Unsupported parameter file extension.") 893 | 894 | self.I = np.array([[self.params["Jx"], 0, -self.params["Jxz"]], 895 | [0, self.params["Jy"], 0, ], 896 | [-self.params["Jxz"], 0, self.params["Jz"]] 897 | ]) 898 | 899 | self.gammas = [self.I[0, 0] * self.I[2, 2] - self.I[0, 2] ** 2] 900 | self.gammas.append((np.abs(self.I[0, 2]) * (self.I[0, 0] - self.I[1, 1] + self.I[2, 2])) / self.gammas[0]) 901 | self.gammas.append((self.I[2, 2] * (self.I[2, 2] - self.I[1, 1]) + self.I[0, 2] ** 2) / self.gammas[0]) 902 | self.gammas.append(self.I[2, 2] / self.gammas[0]) 903 | self.gammas.append(np.abs(self.I[0, 2]) / self.gammas[0]) 904 | self.gammas.append((self.I[2, 2] - self.I[0, 0]) / self.I[1, 1]) 905 | self.gammas.append(np.abs(self.I[0, 2]) / self.I[1, 1]) 906 | self.gammas.append(((self.I[0, 0] - self.I[1, 1]) * self.I[0, 0] + self.I[0, 2] ** 2) / self.gammas[0]) 907 | self.gammas.append(self.I[0, 0] / self.gammas[0]) 908 | 909 | self.params["ar"] = self.params["b"] ** 2 / self.params["S_wing"] # aspect ratio 910 | 911 | with open(config_path) as config_file: 912 | self.cfg = json.load(config_file) 913 | 914 | if config_kw is not None: 915 | set_config_attrs(self.cfg, config_kw) 916 | 917 | self.state = {} 918 | self.attitude_states = ["roll", "pitch", "yaw"] 919 | self.actuator_states = ["elevator", "aileron", "rudder", "throttle", "elevon_left", "elevon_right"] 920 | self.model_inputs = ["elevator", "aileron", "rudder", "throttle"] 921 | self.energy_states = [] 922 | 923 | if not set(self.REQUIRED_VARIABLES).issubset([v["name"] for v in self.cfg["variables"]]): 924 | raise Exception("Missing required variable(s) in config file: {}".format( 925 | ",".join(list(set(self.REQUIRED_VARIABLES) - set([v["name"] for v in self.cfg["variables"]]))))) 926 | 927 | self.dt = self.cfg["dt"] 928 | self.rho = self.cfg["rho"] 929 | self.g = self.cfg["g"] 930 | self.wind = Wind(mag_min=self.cfg["wind_magnitude_min"], mag_max=self.cfg["wind_magnitude_max"], 931 | turbulence=self.cfg["turbulence"], turbulence_intensity=self.cfg["turbulence_intensity"], 932 | sim_length=self.cfg.get("turbulence_sim_length", 300), 933 | dt=self.cfg["dt"], b=self.params["b"]) 934 | 935 | self.state["attitude"] = AttitudeQuaternion() 936 | self.attitude_states_with_constraints = [] 937 | 938 | self.actuation = Actuation(model_inputs=self.model_inputs, 939 | actuator_inputs=self.cfg["actuation"]["inputs"], 940 | dynamics=self.cfg["actuation"]["dynamics"]) 941 | 942 | for v in self.cfg["variables"]: 943 | if v["name"] in self.attitude_states and any([v.get(attribute, None) is not None for attribute in 944 | ["constraint_min", "constraint_max", "value_min", 945 | "value_max"]]): 946 | self.attitude_states_with_constraints.append(v["name"]) 947 | if v["name"] in self.actuator_states: 948 | self.state[v["name"]] = ControlVariable(**v) 949 | self.actuation.add_state(self.state[v["name"]]) 950 | elif "energy" in v["name"]: 951 | self.energy_states.append(v["name"]) 952 | self.state[v["name"]] = EnergyVariable(mass=self.params["mass"], inertia_matrix=self.I, gravity=self.g, **v) 953 | else: 954 | self.state[v["name"]] = Variable(**v) 955 | 956 | if "wind" in v["name"]: 957 | self.wind.components.append(self.state[v["name"]]) 958 | 959 | for state in self.model_inputs: 960 | if state not in self.state: 961 | self.state[state] = ControlVariable(name=state, disabled=True) 962 | self.actuation.add_state(self.state[state]) 963 | 964 | self.actuation.finalize() 965 | 966 | for energy_state in self.energy_states: 967 | for req_var_name in self.state[energy_state].required_variables: 968 | self.state[energy_state].add_requirement(req_var_name, self.state[req_var_name]) 969 | 970 | # TODO: check that all plotted variables are declared in cfg.variables 971 | self.plots = [] 972 | for i, p in enumerate(self.cfg["plots"]): 973 | vars = p.pop("states") 974 | p["dt"] = self.dt 975 | p["id"] = i 976 | self.plots.append(Plot(**p)) 977 | 978 | for v_name in vars: 979 | self.plots[-1].add_variable(self.state[v_name]) 980 | 981 | self.cur_sim_step = None 982 | self.viewer = None 983 | 984 | def seed(self, seed=None): 985 | """ 986 | Seed the random number generator of the flight simulator 987 | 988 | :param seed: (int) seed for random state 989 | """ 990 | for i, var in enumerate(self.state.values()): 991 | var.seed(seed + i) 992 | 993 | self.wind.seed(seed) 994 | 995 | def reset(self, state=None, turbulence_noise=None): 996 | """ 997 | Reset state of simulator. Must be called before first use. 998 | 999 | :param state: (dict) set initial value of states to given value. 1000 | """ 1001 | self.cur_sim_step = 0 1002 | 1003 | for name, var in self.state.items(): 1004 | if name in ["Va", "alpha", "beta", "attitude"] or "wind" in name or "energy" in name or isinstance(var, ControlVariable): 1005 | continue 1006 | var_init = state[name] if state is not None and name in state else None 1007 | var.reset(value=var_init) 1008 | 1009 | self.actuation.reset(state) 1010 | 1011 | wind_init = None 1012 | if state is not None: 1013 | if "wind" in state: 1014 | wind_init = state["wind"] 1015 | elif all([comp in state for comp in ["wind_n", "wind_e", "wind_d"]]): 1016 | wind_init = [state["wind_n"], state["wind_e"], state["wind_d"]] 1017 | self.wind.reset(wind_init, turbulence_noise) 1018 | 1019 | Theta = self.get_states_vector(["roll", "pitch", "yaw"]) 1020 | vel = np.array(self.get_states_vector(["velocity_u", "velocity_v", "velocity_w"])) 1021 | 1022 | Va, alpha, beta = self._calculate_airspeed_factors(Theta, vel) 1023 | self.state["Va"].reset(Va) 1024 | self.state["alpha"].reset(alpha) 1025 | self.state["beta"].reset(beta) 1026 | 1027 | self.state["attitude"].reset(Theta) 1028 | 1029 | for energy_state in self.energy_states: 1030 | self.state[energy_state].reset(self.state[energy_state].calculate_value()) 1031 | 1032 | def render(self, mode="plot", close=False, viewer=None, targets=None, block=False): 1033 | """ 1034 | Visualize history of simulator states. 1035 | 1036 | :param mode: (str) render mode, one of plot for graph representation and animation for 3D animation with blender 1037 | :param close: (bool) close figure after showing 1038 | :param viewer: (dict) viewer object with figure and gridspec that pyfly will attach plots to 1039 | :param targets: (dict) string list pairs of states with target values added to plots containing these states. 1040 | """ 1041 | if mode == "plot": 1042 | if viewer is not None: 1043 | self.viewer = viewer 1044 | elif self.viewer is None: 1045 | self.viewer = {"fig": plt.figure(figsize=(9, 16))} 1046 | subfig_count = len(self.plots) 1047 | self.viewer["gs"] = matplotlib.gridspec.GridSpec(subfig_count, 1) 1048 | 1049 | for i, p in enumerate(self.plots): 1050 | sub_fig = self.viewer["gs"][i, 0] if p.axis is None else None 1051 | if targets is not None: 1052 | plot_variables = [v.name for v in p.variables] 1053 | plot_targets = list(set(targets).intersection(plot_variables)) 1054 | p.plot(fig=sub_fig, targets={k: v for k, v in targets.items() if k in plot_targets}) 1055 | else: 1056 | p.plot(fig=sub_fig) 1057 | 1058 | if viewer is None: 1059 | plt.show(block=block) 1060 | if close: 1061 | for p in self.plots: 1062 | p.close() 1063 | self.viewer = None 1064 | elif mode == "animation": 1065 | raise NotImplementedError 1066 | else: 1067 | raise ValueError("Unexpected value {} for mode".format(mode)) 1068 | 1069 | def step(self, commands): 1070 | """ 1071 | Perform one integration step from t to t + dt. 1072 | 1073 | :param commands: ([float]) actuator setpoints 1074 | :return: (bool, dict) whether integration step was successfully performed, reason for step failure 1075 | """ 1076 | success = True 1077 | info = {} 1078 | 1079 | # Record command history and apply conditions on actuator setpoints 1080 | control_inputs = self.actuation.set_and_constrain_commands(commands) 1081 | 1082 | y0 = list(self.state["attitude"].value) 1083 | y0.extend(self.get_states_vector(["omega_p", "omega_q", "omega_r", "position_n", "position_e", "position_d", 1084 | "velocity_u", "velocity_v", "velocity_w"])) 1085 | y0.extend(self.actuation.get_values()) 1086 | y0 = np.array(y0) 1087 | 1088 | try: 1089 | sol = scipy.integrate.solve_ivp(fun=lambda t, y: self._dynamics(t, y), t_span=(0, self.dt), 1090 | y0=y0) 1091 | self._set_states_from_ode_solution(sol.y[:, -1], save=True) 1092 | 1093 | Theta = self.get_states_vector(["roll", "pitch", "yaw"]) 1094 | vel = np.array(self.get_states_vector(["velocity_u", "velocity_v", "velocity_w"])) 1095 | 1096 | Va, alpha, beta = self._calculate_airspeed_factors(Theta, vel) 1097 | self.state["Va"].set_value(Va) 1098 | self.state["alpha"].set_value(alpha) 1099 | self.state["beta"].set_value(beta) 1100 | 1101 | for energy_state in self.energy_states: 1102 | self.state[energy_state].set_value(self.state[energy_state].calculate_value(), save=True) 1103 | 1104 | self.wind.set_value(self.cur_sim_step) 1105 | except ConstraintException as e: 1106 | success = False 1107 | info = {"termination": e.variable} 1108 | 1109 | self.cur_sim_step += 1 1110 | 1111 | return success, info 1112 | 1113 | def get_states_vector(self, states, attribute="value"): 1114 | """ 1115 | Get attribute of multiple states. 1116 | 1117 | :param states: ([string]) list of state names 1118 | :param attribute: (string) state attribute to retrieve 1119 | :return: ([?]) list of attribute for each state 1120 | """ 1121 | return [getattr(self.state[state_name], attribute) for state_name in states] 1122 | 1123 | def save_history(self, path, states): 1124 | """ 1125 | Save simulator state history to file. 1126 | 1127 | :param path: (string) path to save history to 1128 | :param states: (string or [string]) names of states to save 1129 | """ 1130 | res = {} 1131 | if states == "all": 1132 | save_states = self.state.keys() 1133 | else: 1134 | save_states = states 1135 | 1136 | for state in save_states: 1137 | res[state] = self.state[state].history 1138 | 1139 | np.save(path, res) 1140 | 1141 | def _dynamics(self, t, y, control_sp=None): 1142 | """ 1143 | Right hand side of dynamics differential equation. 1144 | 1145 | :param t: (float) current integration time 1146 | :param y: ([float]) current values of integration states 1147 | :param control_sp: ([float]) setpoints for actuators 1148 | :return: ([float]) right hand side of differential equations 1149 | """ 1150 | 1151 | if t > 0: 1152 | self._set_states_from_ode_solution(y, save=False) 1153 | 1154 | attitude = y[:4] 1155 | 1156 | omega = self.get_states_vector(["omega_p", "omega_q", "omega_r"]) 1157 | vel = np.array(self.get_states_vector(["velocity_u", "velocity_v", "velocity_w"])) 1158 | u_states = self.get_states_vector(self.model_inputs) 1159 | 1160 | f, tau = self._forces(attitude, omega, vel, u_states) 1161 | 1162 | return np.concatenate([ 1163 | self._f_attitude_dot(t, attitude, omega), 1164 | self._f_omega_dot(t, omega, tau), 1165 | self._f_p_dot(t, vel, attitude), 1166 | self._f_v_dot(t, vel, omega, f), 1167 | self._f_u_dot(t, control_sp) 1168 | ]) 1169 | 1170 | def _forces(self, attitude, omega, vel, controls): 1171 | """ 1172 | Get aerodynamic forces acting on aircraft. 1173 | 1174 | :param attitude: ([float]) attitude quaternion of aircraft 1175 | :param omega: ([float]) angular velocity of aircraft 1176 | :param vel: ([float]) linear velocity of aircraft 1177 | :param controls: ([float]) state of actutators 1178 | :return: ([float], [float]) forces and moments in x, y, z of aircraft frame 1179 | """ 1180 | elevator, aileron, rudder, throttle = controls 1181 | 1182 | p, q, r = omega 1183 | 1184 | if self.wind.turbulence: 1185 | p_w, q_w, r_w = self.wind.get_turbulence_angular(self.cur_sim_step) 1186 | p, q, r = p - p_w, q - q_w, r - r_w 1187 | 1188 | Va, alpha, beta = self._calculate_airspeed_factors(attitude, vel) 1189 | 1190 | Va = self.state["Va"].apply_conditions(Va) 1191 | alpha = self.state["alpha"].apply_conditions(alpha) 1192 | beta = self.state["beta"].apply_conditions(beta) 1193 | 1194 | pre_fac = 0.5 * self.rho * Va ** 2 * self.params["S_wing"] 1195 | 1196 | e0, e1, e2, e3 = attitude 1197 | fg_b = self.params["mass"] * self.g * np.array([2 * (e1 * e3 - e2 * e0), 1198 | 2 * (e2 * e3 + e1 * e0), 1199 | e3 ** 2 + e0 ** 2 - e1 ** 2 - e2 ** 2]) 1200 | 1201 | C_L_alpha_lin = self.params["C_L_0"] + self.params["C_L_alpha"] * alpha 1202 | 1203 | # Nonlinear version of lift coefficient with stall 1204 | a_0 = self.params["a_0"] 1205 | M = self.params["M"] 1206 | e = self.params["e"] # oswald efficiency 1207 | ar = self.params["ar"] 1208 | C_D_p = self.params["C_D_p"] 1209 | C_m_fp = self.params["C_m_fp"] 1210 | C_m_alpha = self.params["C_m_alpha"] 1211 | C_m_0 = self.params["C_m_0"] 1212 | 1213 | sigma = (1 + np.exp(-M * (alpha - a_0)) + np.exp(M * (alpha + a_0))) / ( 1214 | (1 + np.exp(-M * (alpha - a_0))) * (1 + np.exp(M * (alpha + a_0)))) 1215 | C_L_alpha = (1 - sigma) * C_L_alpha_lin + sigma * (2 * np.sign(alpha) * np.sin(alpha) ** 2 * np.cos(alpha)) 1216 | 1217 | f_lift_s = pre_fac * (C_L_alpha + self.params["C_L_q"] * self.params["c"] / (2 * Va) * q + self.params[ 1218 | "C_L_delta_e"] * elevator) 1219 | # C_D_alpha = self.params["C_D_0"] + self.params["C_D_alpha1"] * alpha + self.params["C_D_alpha2"] * alpha ** 2 1220 | C_D_alpha = C_D_p + (1 - sigma) * (self.params["C_L_0"] + self.params["C_L_alpha"] * alpha) ** 2 / ( 1221 | np.pi * e * ar) + sigma * (2 * np.sign(alpha) * math.pow(np.sin(alpha), 3)) 1222 | C_D_beta = self.params["C_D_beta1"] * beta + self.params["C_D_beta2"] * beta ** 2 1223 | f_drag_s = pre_fac * ( 1224 | C_D_alpha + C_D_beta + self.params["C_D_q"] * self.params["c"] / (2 * Va) * q + self.params[ 1225 | "C_D_delta_e"] * elevator ** 2) 1226 | 1227 | C_m = (1 - sigma) * (C_m_0 + C_m_alpha * alpha) + sigma * (C_m_fp * np.sign(alpha) * np.sin(alpha) ** 2) 1228 | m = pre_fac * self.params["c"] * (C_m + self.params["C_m_q"] * self.params["b"] / (2 * Va) * q + self.params[ 1229 | "C_m_delta_e"] * elevator) 1230 | 1231 | f_y = pre_fac * ( 1232 | self.params["C_Y_0"] + self.params["C_Y_beta"] * beta + self.params["C_Y_p"] * self.params["b"] / ( 1233 | 2 * Va) * p + self.params["C_Y_r"] * self.params["b"] / (2 * Va) * r + self.params[ 1234 | "C_Y_delta_a"] * aileron + self.params["C_Y_delta_r"] * rudder) 1235 | l = pre_fac * self.params["b"] * ( 1236 | self.params["C_l_0"] + self.params["C_l_beta"] * beta + self.params["C_l_p"] * self.params["b"] / ( 1237 | 2 * Va) * p + self.params["C_l_r"] * self.params["b"] / (2 * Va) * r + self.params[ 1238 | "C_l_delta_a"] * aileron + self.params["C_l_delta_r"] * rudder) 1239 | n = pre_fac * self.params["b"] * ( 1240 | self.params["C_n_0"] + self.params["C_n_beta"] * beta + self.params["C_n_p"] * self.params["b"] / ( 1241 | 2 * Va) * p + self.params["C_n_r"] * self.params["b"] / (2 * Va) * r + self.params[ 1242 | "C_n_delta_a"] * aileron + self.params["C_n_delta_r"] * rudder) 1243 | 1244 | f_aero = np.dot(self._rot_b_v(np.array([0, alpha, beta])), np.array([-f_drag_s, f_y, -f_lift_s])) 1245 | tau_aero = np.array([l, m, n]) 1246 | 1247 | Vd = Va + throttle * (self.params["k_motor"] - Va) 1248 | f_prop = np.array([0.5 * self.rho * self.params["S_prop"] * self.params["C_prop"] * Vd * (Vd - Va), 0, 0]) 1249 | tau_prop = np.array([-self.params["k_T_P"] * (self.params["k_Omega"] * throttle) ** 2, 0, 0]) 1250 | 1251 | f = f_prop + fg_b + f_aero 1252 | tau = tau_aero + tau_prop 1253 | 1254 | return f, tau 1255 | 1256 | def _f_attitude_dot(self, t, attitude, omega): 1257 | """ 1258 | Right hand side of quaternion attitude differential equation. 1259 | 1260 | :param t: (float) time of integration 1261 | :param attitude: ([float]) attitude quaternion 1262 | :param omega: ([float]) angular velocity 1263 | :return: ([float]) right hand side of quaternion attitude differential equation. 1264 | """ 1265 | p, q, r = omega 1266 | T = np.array([[0, -p, -q, -r], 1267 | [p, 0, r, -q], 1268 | [q, -r, 0, p], 1269 | [r, q, -p, 0] 1270 | ]) 1271 | return 0.5 * np.dot(T, attitude) 1272 | 1273 | def _f_omega_dot(self, t, omega, tau): 1274 | """ 1275 | Right hand side of angular velocity differential equation. 1276 | 1277 | :param t: (float) time of integration 1278 | :param omega: ([float]) angular velocity 1279 | :param tau: ([float]) moments acting on aircraft 1280 | :return: ([float]) right hand side of angular velocity differential equation. 1281 | """ 1282 | return np.array([ 1283 | self.gammas[1] * omega[0] * omega[1] - self.gammas[2] * omega[1] * omega[2] + self.gammas[3] * tau[0] + 1284 | self.gammas[4] * tau[2], 1285 | self.gammas[5] * omega[0] * omega[2] - self.gammas[6] * (omega[0] ** 2 - omega[2] ** 2) + tau[1] / self.I[ 1286 | 1, 1], 1287 | self.gammas[7] * omega[0] * omega[1] - self.gammas[1] * omega[1] * omega[2] + self.gammas[4] * tau[0] + 1288 | self.gammas[8] * tau[2] 1289 | ]) 1290 | 1291 | def _f_v_dot(self, t, v, omega, f): 1292 | """ 1293 | Right hand side of linear velocity differential equation. 1294 | 1295 | :param t: (float) time of integration 1296 | :param v: ([float]) linear velocity 1297 | :param omega: ([float]) angular velocity 1298 | :param f: ([float]) forces acting on aircraft 1299 | :return: ([float]) right hand side of linear velocity differntial equation. 1300 | """ 1301 | v_dot = np.array([ 1302 | omega[2] * v[1] - omega[1] * v[2] + f[0] / self.params["mass"], 1303 | omega[0] * v[2] - omega[2] * v[0] + f[1] / self.params["mass"], 1304 | omega[1] * v[0] - omega[0] * v[1] + f[2] / self.params["mass"] 1305 | ]) 1306 | 1307 | return v_dot 1308 | 1309 | def _f_p_dot(self, t, v, attitude): 1310 | """ 1311 | Right hand side of position differential equation. 1312 | 1313 | :param t: (float) time of integration 1314 | :param v: ([float]) linear velocity 1315 | :param attitude: ([float]) attitude quaternion 1316 | :return: ([float]) right hand side of position differntial equation. 1317 | """ 1318 | e0, e1, e2, e3 = attitude 1319 | T = np.array([[e1 ** 2 + e0 ** 2 - e2 ** 2 - e3 ** 2, 2 * (e1 * e2 - e3 * e0), 2 * (e1 * e3 + e2 * e0)], 1320 | [2 * (e1 * e2 + e3 * e0), e2 ** 2 + e0 ** 2 - e1 ** 2 - e3 ** 2, 2 * (e2 * e3 - e1 * e0)], 1321 | [2 * (e1 * e3 - e2 * e0), 2 * (e2 * e3 + e1 * e0), e3 ** 2 + e0 ** 2 - e1 ** 2 - e2 ** 2] 1322 | ]) 1323 | return np.dot(T, v) 1324 | 1325 | def _f_u_dot(self, t, setpoints): 1326 | """ 1327 | Right hand side of actuator differential equation. 1328 | 1329 | :param t: (float) time of integration 1330 | :param setpoints: ([float]) setpoint for actuators 1331 | :return: ([float]) right hand side of actuator differential equation. 1332 | """ 1333 | return self.actuation.rhs(setpoints) 1334 | 1335 | def _rot_b_v(self, attitude): 1336 | """ 1337 | Rotate vector from body frame to vehicle frame. 1338 | 1339 | :param Theta: ([float]) vector to rotate, either as Euler angles or quaternion 1340 | :return: ([float]) rotated vector 1341 | """ 1342 | if len(attitude) == 3: 1343 | phi, th, psi = attitude 1344 | return np.array([ 1345 | [np.cos(th) * np.cos(psi), np.cos(th) * np.sin(psi), -np.sin(th)], 1346 | [np.sin(phi) * np.sin(th) * np.cos(psi) - np.cos(phi) * np.sin(psi), 1347 | np.sin(phi) * np.sin(th) * np.sin(psi) + np.cos(phi) * np.cos(psi), np.sin(phi) * np.cos(th)], 1348 | [np.cos(phi) * np.sin(th) * np.cos(psi) + np.sin(phi) * np.sin(psi), 1349 | np.cos(phi) * np.sin(th) * np.sin(psi) - np.sin(phi) * np.cos(psi), np.cos(phi) * np.cos(th)] 1350 | ]) 1351 | elif len(attitude) == 4: 1352 | e0, e1, e2, e3 = attitude 1353 | return np.array([[-1 + 2 * (e0 ** 2 + e1 ** 2), 2 * (e1 * e2 + e3 * e0), 2 * (e1 * e3 - e2 * e0)], 1354 | [2 * (e1 * e2 - e3 * e0), -1 + 2 * (e0 ** 2 + e2 ** 2), 2 * (e2 * e3 + e1 * e0)], 1355 | [2 * (e1 * e3 + e2 * e0), 2 * (e2 * e3 - e1 * e0), -1 + 2 * (e0 ** 2 + e3 ** 2)]]) 1356 | 1357 | else: 1358 | raise ValueError("Attitude is neither Euler angles nor Quaternion") 1359 | 1360 | def _rot_v_b(self, Theta): 1361 | """ 1362 | Rotate vector from vehicle frame to body frame. 1363 | 1364 | :param Theta: ([float]) vector to rotate 1365 | :return: ([float]) rotated vector 1366 | """ 1367 | phi, th, psi = Theta 1368 | return np.array([ 1369 | [np.cos(th) * np.cos(psi), np.sin(phi) * np.sin(th) * np.cos(psi) - np.cos(phi) * np.sin(psi), 1370 | np.cos(phi) * np.sin(th) * np.cos(psi) + np.sin(phi) * np.sin(psi)], 1371 | [np.cos(th) * np.sin(psi), np.sin(phi) * np.sin(th) * np.sin(psi) + np.cos(phi) * np.cos(psi), 1372 | np.cos(phi) * np.sin(th) * np.sin(psi) - np.sin(phi) * np.cos(psi)], 1373 | [-np.sin(th), np.sin(phi) * np.cos(th), np.cos(phi) * np.cos(th)] 1374 | ]) 1375 | 1376 | def _calculate_airspeed_factors(self, attitude, vel): 1377 | """ 1378 | Calculate the airspeed factors airspeed (Va), angle of attack (alpha) and sideslip angle (beta). 1379 | 1380 | :param attitude: ([float]) attitude quaternion 1381 | :param vel: ([float]) linear velocity 1382 | :return: ([float]) airspeed factors Va, alpha, beta 1383 | """ 1384 | if self.wind.turbulence: 1385 | turbulence = self.wind.get_turbulence_linear(self.cur_sim_step) 1386 | else: 1387 | turbulence = np.zeros(3) 1388 | 1389 | wind_vec = np.dot(self._rot_b_v(attitude), self.wind.steady) + turbulence 1390 | airspeed_vec = vel - wind_vec 1391 | 1392 | Va = np.linalg.norm(airspeed_vec) 1393 | alpha = np.arctan2(airspeed_vec[2], airspeed_vec[0]) 1394 | beta = np.arcsin(airspeed_vec[1] / Va) 1395 | 1396 | return Va, alpha, beta 1397 | 1398 | def _set_states_from_ode_solution(self, ode_sol, save): 1399 | """ 1400 | Set states from ODE solution vector. 1401 | 1402 | :param ode_sol: ([float]) solution vector from ODE solver 1403 | :param save: (bool) whether to save values to state history, i.e. whether solution represents final step 1404 | solution or intermediate values during integration. 1405 | """ 1406 | self.state["attitude"].set_value(ode_sol[:4] / np.linalg.norm(ode_sol[:4])) 1407 | if save: 1408 | euler_angles = self.state["attitude"].as_euler_angle() 1409 | self.state["roll"].set_value(euler_angles["roll"], save=save) 1410 | self.state["pitch"].set_value(euler_angles["pitch"], save=save) 1411 | self.state["yaw"].set_value(euler_angles["yaw"], save=save) 1412 | else: 1413 | for state in self.attitude_states_with_constraints: 1414 | self.state[state].set_value(self.state["attitude"].as_euler_angle(state)) 1415 | start_i = 4 1416 | 1417 | self.state["omega_p"].set_value(ode_sol[start_i], save=save) 1418 | self.state["omega_q"].set_value(ode_sol[start_i + 1], save=save) 1419 | self.state["omega_r"].set_value(ode_sol[start_i + 2], save=save) 1420 | self.state["position_n"].set_value(ode_sol[start_i + 3], save=save) 1421 | self.state["position_e"].set_value(ode_sol[start_i + 4], save=save) 1422 | self.state["position_d"].set_value(ode_sol[start_i + 5], save=save) 1423 | self.state["velocity_u"].set_value(ode_sol[start_i + 6], save=save) 1424 | self.state["velocity_v"].set_value(ode_sol[start_i + 7], save=save) 1425 | self.state["velocity_w"].set_value(ode_sol[start_i + 8], save=save) 1426 | self.actuation.set_states(ode_sol[start_i + 9:], save=save) 1427 | 1428 | 1429 | if __name__ == "__main__": 1430 | from dryden import DrydenGustModel 1431 | from pid_controller import PIDController 1432 | 1433 | pfly = PyFly("pyfly_config.json", "x8_param.mat") 1434 | pfly.seed(0) 1435 | 1436 | pid = PIDController(pfly.dt) 1437 | pid.set_reference(phi=0.2, theta=0, va=22) 1438 | 1439 | pfly.reset(state={"roll": -0.5, "pitch": 0.15}) 1440 | 1441 | for i in range(500): 1442 | phi = pfly.state["roll"].value 1443 | theta = pfly.state["pitch"].value 1444 | Va = pfly.state["Va"].value 1445 | omega = [pfly.state["omega_p"].value, pfly.state["omega_q"].value, pfly.state["omega_r"].value] 1446 | 1447 | action = pid.get_action(phi, theta, Va, omega) 1448 | success, step_info = pfly.step(action) 1449 | 1450 | if not success: 1451 | break 1452 | 1453 | pfly.render(block=True) 1454 | else: 1455 | from .dryden import DrydenGustModel 1456 | -------------------------------------------------------------------------------- /pyfly/pyfly_config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dt": 0.01, 3 | "rho": 1.2250, 4 | "g": 9.81, 5 | "wind_magnitude_min": 0, 6 | "wind_magnitude_max": 0, 7 | "turbulence": false, 8 | "turbulence_intensity": "light", 9 | "turbulence_sim_length": 300, 10 | "variables": [ 11 | { 12 | "name": "roll", 13 | "convert_to_radians": true, 14 | "init_min": -30, 15 | "init_max": 30, 16 | "unit": "degrees", 17 | "wrap": true, 18 | "label": "$\\phi$" 19 | }, 20 | { 21 | "name": "pitch", 22 | "convert_to_radians": true, 23 | "unit": "degrees", 24 | "init_min": -30, 25 | "init_max": 30, 26 | "label": "$\\theta$" 27 | }, 28 | { 29 | "name": "yaw", 30 | "convert_to_radians": true, 31 | "unit": "degrees", 32 | "init_min": -30, 33 | "init_max": 30, 34 | "wrap": true, 35 | "label": "$\\psi$" 36 | }, 37 | { 38 | "name": "omega_p", 39 | "init_min": -40, 40 | "init_max": 40, 41 | "constraint_min": -180, 42 | "constraint_max": 180, 43 | "convert_to_radians": true, 44 | "unit": "degrees/s", 45 | "label": "p" 46 | }, 47 | { 48 | "name": "omega_q", 49 | "init_min": -40, 50 | "init_max": 40, 51 | "constraint_min": -180, 52 | "constraint_max": 180, 53 | "convert_to_radians": true, 54 | "unit": "degrees/s", 55 | "label": "q" 56 | }, 57 | { 58 | "name": "omega_r", 59 | "init_min": -40, 60 | "init_max": 40, 61 | "constraint_min": -180, 62 | "constraint_max": 180, 63 | "convert_to_radians": true, 64 | "unit": "degrees/s", 65 | "label": "r" 66 | }, 67 | { 68 | "name": "position_n", 69 | "init_min": -100, 70 | "init_max": 100, 71 | "unit": "m", 72 | "label": "North" 73 | }, 74 | { 75 | "name": "position_e", 76 | "init_min": -100, 77 | "init_max": 100, 78 | "unit": "m", 79 | "label": "East" 80 | }, 81 | { 82 | "name": "position_d", 83 | "init_min": -20, 84 | "init_max": -100, 85 | "unit": "m", 86 | "label": "Down" 87 | }, 88 | { 89 | "name": "velocity_u", 90 | "init_min": 13, 91 | "init_max": 22, 92 | "unit": "m/s", 93 | "label": "u" 94 | }, 95 | { 96 | "name": "velocity_v", 97 | "init_min": -5, 98 | "init_max": 5, 99 | "unit": "m/s", 100 | "label": "v" 101 | }, 102 | { 103 | "name": "velocity_w", 104 | "init_min": -5, 105 | "init_max": 5, 106 | "unit": "m/s", 107 | "label": "w" 108 | }, 109 | { 110 | "name": "alpha", 111 | "unit": "degrees", 112 | "label": "$\\alpha$" 113 | }, 114 | { 115 | "name": "beta", 116 | "convert_to_radians": true, 117 | "unit": "degrees", 118 | "label": "$\\beta$" 119 | }, 120 | { 121 | "name": "Va", 122 | "value_min": 1e-6, 123 | "unit": "m/s", 124 | "label": "$V_a$" 125 | }, 126 | { 127 | "name": "wind_n", 128 | "init_min": 8, 129 | "init_max": 8, 130 | "unit": "m/s", 131 | "label": "North" 132 | }, 133 | { 134 | "name": "wind_e", 135 | "init_min": 8, 136 | "init_max": 8, 137 | "unit": "m/s", 138 | "label": "East" 139 | }, 140 | { 141 | "name": "wind_d", 142 | "init_min": 8, 143 | "init_max": 8, 144 | "unit": "m/s", 145 | "label": "Down" 146 | }, 147 | { 148 | "name": "elevon_left", 149 | "value_min": -30, 150 | "init_min": 0, 151 | "init_max": 0, 152 | "value_max": 35, 153 | "convert_to_radians": true, 154 | "order": 2, 155 | "dot_max": 3.4907, 156 | "omega_0": 100, 157 | "zeta": 1.71, 158 | "unit": "%", 159 | "label": "elevon_l" 160 | }, 161 | { 162 | "name": "elevon_right", 163 | "value_min": -30, 164 | "init_min": 0, 165 | "init_max": 0, 166 | "value_max": 35, 167 | "convert_to_radians": true, 168 | "order": 2, 169 | "dot_max": 3.4907, 170 | "omega_0": 100, 171 | "zeta": 1.71, 172 | "unit": "%", 173 | "label": "elevon_r" 174 | }, 175 | { 176 | "name": "elevator", 177 | "value_min": -30, 178 | "init_min": 0, 179 | "init_max": 0, 180 | "value_max": 35, 181 | "convert_to_radians": true, 182 | "order": 2, 183 | "dot_max": 3.4907, 184 | "omega_0": 100, 185 | "zeta": 1.71, 186 | "disabled": true, 187 | "unit": "%", 188 | "label": "elevator" 189 | }, 190 | { 191 | "name": "aileron", 192 | "value_min": -30, 193 | "init_min": 0, 194 | "init_max": 0, 195 | "value_max": 35, 196 | "convert_to_radians": true, 197 | "order": 2, 198 | "dot_max": 3.4907, 199 | "omega_0": 100, 200 | "zeta": 1.71, 201 | "disabled": true, 202 | "unit": "%", 203 | "label": "aileron" 204 | }, 205 | { 206 | "name": "rudder", 207 | "dot_max": 3.4907, 208 | "convert_to_radians": true, 209 | "order": 2, 210 | "omega_0": 100, 211 | "zeta": 1.71, 212 | "disabled": true, 213 | "unit": "%", 214 | "label": "rudder" 215 | }, 216 | { 217 | "name": "throttle", 218 | "value_min": 0, 219 | "value_max": 1, 220 | "init_min": 0, 221 | "init_max": 0, 222 | "order": 1, 223 | "tau": 0.2, 224 | "unit": "%", 225 | "label": "throttle" 226 | }, 227 | { 228 | "name": "energy_total", 229 | "unit": "J", 230 | "label": "$E_T$" 231 | }, 232 | { 233 | "name": "energy_potential", 234 | "unit": "J", 235 | "label": "$E_p$" 236 | }, 237 | { 238 | "name": "energy_kinetic_rotational", 239 | "unit": "J", 240 | "label": "$E_{K_R}$" 241 | }, 242 | { 243 | "name": "energy_kinetic_translational", 244 | "unit": "J", 245 | "label": "$E_{K_T}$" 246 | } 247 | ], 248 | "actuation": { 249 | "dynamics": ["elevon_right", "elevon_left", "throttle"], 250 | "inputs": ["elevator", "aileron", "throttle"] 251 | }, 252 | "plots": [ 253 | { 254 | "title": "Roll ($\\phi$) and Pitch ($\\theta$)", 255 | "x_unit": "seconds", 256 | "states": ["roll", "pitch"] 257 | }, 258 | { 259 | "title": "Angular rates ($\\omega$)", 260 | "x_unit": "seconds", 261 | "states": ["omega_p", "omega_q", "omega_r"] 262 | }, 263 | { 264 | "title": "Angle of attack ($\\alpha$), sideslip angle ($\\beta$) and airspeed ($V_a$)", 265 | "x_unit": "seconds", 266 | "states": ["alpha", "beta", "Va"] 267 | }, 268 | { 269 | "title": "Control States", 270 | "x_unit": "seconds", 271 | "states": ["elevator", "aileron", "throttle"], 272 | "plot_quantity": "value" 273 | }, 274 | { 275 | "title": "Control commands", 276 | "x_unit": "seconds", 277 | "states": ["elevator", "aileron", "throttle"], 278 | "plot_quantity": "command" 279 | }, 280 | { 281 | "title": "Energy", 282 | "x_unit": "timesteps", 283 | "states": ["energy_total", "energy_kinetic_translational", "energy_potential"] 284 | } 285 | ] 286 | } -------------------------------------------------------------------------------- /pyfly/x8_param.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/eivindeb/pyfly/8451687b8df4a7bc6f78f3a1cdcfd503b76bf596/pyfly/x8_param.mat -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [metadata] 2 | license-file = LICENSE 3 | description-file = README.md 4 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup( 4 | name='pyfly-fixed-wing', 5 | version='0.1.2', 6 | url="https://github.com/eivindeb/pyfly", 7 | author="Eivind Bøhn", 8 | author_email="eivind.bohn@gmail.com", 9 | description="Python Fixed-Wing Flight Simulator", 10 | packages=["pyfly"], 11 | package_data={"pyfly": ["x8_param.mat", "pyfly_config.json"]}, 12 | license='MIT', 13 | long_description_content_type='text/markdown', 14 | long_description=open('README.md').read(), 15 | install_requires=[ 16 | "cycler>=0.10.0", 17 | "kiwisolver>=1.1.0", 18 | "matplotlib>=3.1.0", 19 | "numpy>=1.16.4", 20 | "pyparsing>=2.4.0", 21 | "python-dateutil>=2.8.0", 22 | "scipy>=1.3.0", 23 | "six>=1.12.0" 24 | ], 25 | ) --------------------------------------------------------------------------------