Symmetric glider v1

airplane/__init__.py

@@ -0,0 +1,8 @@
+from gymnasium.envs.registration import register
+
+# Register the first custom environment
+register(
+    id='ReducedSymmetricGliderPullout-v0',
+    entry_point='airplane.reduced_symmetric_glider_pullout:ReducedSymmetricGliderPullout', 
+    max_episode_steps=100, 
+)

airplane/airplane_env.py

@@ -0,0 +1,47 @@
+import numpy as np
+from gymnasium import Env
+
+class AirplaneEnv(Env):
+    metadata = {"render_modes": ["human", "ascii", "ansi"], "render_fps": 60}
+    # TODO(gtorre): use render fps
+
+    def __init__(self, airplane, render_mode=None):
+        self.airplane = airplane
+
+        self.visualiser = None
+        self.render_mode = render_mode
+        assert render_mode is None or render_mode in self.metadata["render_modes"]
+        self.window = None
+        self.clock = None
+
+    def seed(self, seed=None):
+        np.random.seed(seed)
+
+    def render(self, mode: str | None = "ascii"):
+        """Renders the environment.
+        :param mode: str, the mode to render with:
+        - human: render to the current display or terminal and
+          return nothing. Usually for human consumption.
+        - ansi: Return a string (str) or StringIO.StringIO containing a
+          terminal-style text representation. The text can include newlines
+          and ANSI escape sequences (e.g. for colors).
+        """
+        if mode == "human":
+            pass
+            #if not self.visualiser:
+                #self.visualiser = Visualizer(self.airplane)
+            #self.visualiser.plot()
+
+        else:  # ANSI or ASCII
+            # TODO: Add additional observations
+            print(
+                f"\u001b[34m Flight Path Angle (deg): {np.rad2deg(self.airplane.flight_path_angle):.2f}\u001b[37m"
+            )
+            # TODO: Proper stall prediction
+            if self.airplane.flight_path_angle > 0.7:
+                print("\u001b[35m -- STALL --\u001b[37m")
+
+    def close(self):
+        if self.window is not None:
+            # close
+            raise NotImplementedError

airplane/grumman.py

@@ -0,0 +1,131 @@
+import numpy as np
+
+
+class Grumman:
+    ####################################
+    ### Grumman American AA-1 Yankee ###
+    ####################################
+    """Base class for airplane parameters"""
+
+    def __init__(self):
+        ######################
+        ### Sim parameters ###
+        ######################
+        self.TIME_STEP = 0.01
+        self.GRAVITY = 9.81
+        self.AIR_DENSITY = 1.225  # Density  (ρ)  [kg/m3]
+
+        ###########################
+        ### Airplane parameters ###
+        ###########################
+        # Aerodynamic model: CL coefficients
+        self.CL_0 = 0.41
+        self.CL_ALPHA = 4.6983
+        self.CL_ELEVATOR = 0.361
+        self.CL_QHAT = 2.42
+        # Aerodynamic model: CD coefficients
+        self.CD_0 = 0.0525
+        self.CD_ALPHA = 0.2068
+        self.CD_ALPHA2 = 1.8712
+        # Aerodynamic model: Cm coefficients
+        self.CM_0 = 0.076
+        self.CM_ALPHA = -0.8938
+        self.CM_ELEVATOR = -1.0313
+        self.CM_QHAT = -7.15
+        # Aerodynamic model: Cl coefficients
+        self.Cl_BETA = -0.1089
+        self.Cl_PHAT = -0.52
+        self.Cl_RHAT = 0.19
+        self.Cl_AILERON = -0.1031
+        self.Cl_RUDDER = 0.0143
+        # Physical model
+        self.MASS = 697.18  # Mass  (m)  [kg]
+        self.WING_SURFACE_AREA = 9.1147  # Wing surface area  (S)  [m2]
+        self.CHORD = 1.22  # Chord  (c)  [m]
+        self.WING_SPAN = 7.46  # Wing Span  (b)  [m]
+        self.I_XX = 808.06   # Inertia  [Kg.m^2]
+        self.I_YY = 1011.43  # Inertia  [Kg.m^2]
+        self.ALPHA_STALL = np.deg2rad(15)  # Stall angle of attack  (αs)  [rad]
+        self.ALPHA_NEGATIVE_STALL = np.deg2rad(-7)  # Negative stall angle of attack  (αs)  [rad]
+        self.CL_STALL = self.CL_0 + self.CL_ALPHA * self.ALPHA_STALL
+        self.CL_REF = self.CL_STALL 
+        # self.STALL_AIRSPEED = 32.19  # Stall air speed  (Vs)  [m/s]
+        self.STALL_AIRSPEED = np.sqrt(self.MASS * self.GRAVITY / (0.5 * self.AIR_DENSITY * \
+                                    self.WING_SURFACE_AREA * self.CL_REF))    # Stall air speed  (Vs)  [m/s]
+        self.MAX_CRUISE_AIRSPEED = 2 * self.STALL_AIRSPEED  # Maximum air speed  (Vs)  [m/s]
+
+        # Throttle model
+        self.THROTTLE_LINEAR_MAPPING = None
+        self._initialize_throttle_model()
+
+    def _update_state_from_derivative(self, value_to_update, value_derivative):
+        value_to_update += self.TIME_STEP * value_derivative
+        return value_to_update
+
+    def _alpha_from_cl(self, c_lift):
+        alpha = (c_lift - self.CL_0) / self.CL_ALPHA
+        return alpha
+
+    def _cl_from_lift_force_and_speed(self, lift_force, airspeed):
+        cl = 2 * lift_force / (self.AIR_DENSITY * self.WING_SURFACE_AREA * airspeed ** 2)
+        return cl
+
+    def _cl_from_alpha(self, alpha, elevator, q_hat):
+        # TODO: review model
+        if alpha <= self.ALPHA_NEGATIVE_STALL:
+            c_lift = self.CL_0 + self.CL_ALPHA * self.ALPHA_NEGATIVE_STALL
+        elif alpha >= self.ALPHA_STALL:
+            # Stall model: Lift saturation
+            c_lift = self.CL_0 + self.CL_ALPHA * self.ALPHA_STALL
+            # Stall model: Lift reduction with opposite slope
+            # c_lift = - self.CL_ALPHA * alpha + self.CL_0 + 2 * self.CL_ALPHA * self.ALPHA_STALL
+        else:
+            c_lift = self.CL_0 + self.CL_ALPHA * alpha + self.CL_ELEVATOR * elevator + self.CL_QHAT * q_hat
+        return c_lift
+
+    def _lift_force_at_speed_and_cl(self, airspeed, lift_coefficient):
+        return 0.5 * self.AIR_DENSITY * self.WING_SURFACE_AREA * airspeed ** 2 * lift_coefficient
+
+    def _cd_from_alpha(self, alpha):
+        c_drag = self.CD_0 + self.CD_ALPHA * alpha + self.CD_ALPHA2 * (alpha ** 2)
+        return c_drag
+
+    def _cd_from_cl(self, c_lift):
+        c_drag = self._cd_from_alpha(self._alpha_from_cl(c_lift))
+        return c_drag
+
+    def _drag_force_at_speed_and_cd(self, airspeed, drag_coefficient):
+        return 0.5 * self.AIR_DENSITY * self.WING_SURFACE_AREA * airspeed ** 2 * drag_coefficient
+
+    def _drag_force_at_cruise_speed(self, airspeed):
+        cruise_lift_force = self.MASS * self.GRAVITY
+        cruise_cl = self._cl_from_lift_force_and_speed(cruise_lift_force, airspeed)
+        alpha = self._alpha_from_cl(cruise_cl)
+        cruise_cd = self._cd_from_alpha(alpha)
+        drag_force = self._drag_force_at_speed_and_cd(airspeed, cruise_cd)
+        return drag_force
+
+    def _rolling_moment_coefficient(self, beta, p_hat, r_hat, aileron, rudder):
+        c_rolling_moment = self.Cl_BETA * beta + self.Cl_PHAT * p_hat + self.Cl_RHAT * r_hat + \
+                 self.Cl_AILERON * aileron + self.Cl_RUDDER * rudder
+        return c_rolling_moment
+
+    def _rolling_moment_at_speed_and_cl(self, airspeed, rolling_moment_coefficient):
+        return 0.5 * self.AIR_DENSITY * self.WING_SURFACE_AREA * self.WING_SPAN * airspeed ** 2 * rolling_moment_coefficient
+
+    def _pitching_moment_coefficient(self, alpha, elevator, q_hat):
+        c_pitch_moment = self.CM_0 + self.CM_ALPHA * alpha + self.CM_ELEVATOR * elevator + self.CM_QHAT * q_hat
+        return c_pitch_moment
+
+    def _pitching_moment_at_speed_and_cm(self, airspeed, pitching_moment_coefficient):
+        return 0.5 * self.AIR_DENSITY * self.WING_SURFACE_AREA * self.CHORD * airspeed ** 2 * pitching_moment_coefficient
+
+    def _initialize_throttle_model(self, ):
+        # Throttle model: Thrust force = Kt * δ_throttle
+        # Max Thrust -> Kt * 1 = Drag(V=Vmax) -> Kt = 0.5 ρ S (Vmax)^2 CD
+        # δ_throttle = 1.0  -> Max Cruise speed: V' = Vmax  ->  V_dot = 0 = Thrust Force - Drag Force
+        self.THROTTLE_LINEAR_MAPPING = self._drag_force_at_cruise_speed(self.MAX_CRUISE_AIRSPEED)
+
+    def _thrust_force_at_throttle(self, throttle):
+        thrust_force = self.THROTTLE_LINEAR_MAPPING * throttle
+        return thrust_force

airplane/reduced_banked_glider_pullout.py

@@ -0,0 +1,57 @@
+import numpy as np
+import gymnasium
+from gymnasium import spaces
+from matplotlib import pyplot as plt
+from airplane.reduced_grumman import ReducedGrumman
+from airplane.airplane_env import AirplaneEnv
+
+
+class ReducedBankedGliderPullout(AirplaneEnv):
+
+    def __init__(self, render_mode=None):
+
+        self.airplane = ReducedGrumman()
+        super().__init__(self.airplane)
+
+        # Observation space: Flight Path Angle (γ), Air Speed (V), Bank Angle (μ)
+        self.observation_space = spaces.Box(np.array([-np.pi, 0.9, np.deg2rad(-20)], np.float32), np.array([0, 4.0, np.deg2rad(200)], np.float32), shape=(3,), dtype=np.float32) 
+        # Action space: Lift Coefficient (CL), Bank Rate (μ')
+        self.action_space = spaces.Box(np.array([-0.5, np.deg2rad(-30)], np.float32), np.array([1.0, np.deg2rad(30)], np.float32), shape=(2,), dtype=np.float32) 
+
+    def _get_obs(self):
+        return np.vstack([self.airplane.flight_path_angle, self.airplane.airspeed_norm, self.airplane.bank_angle], dtype=np.float32).T
+
+    def _get_info(self):
+        return {}
+
+    def reset(self, seed=None, options=None):
+
+        # Choose the initial agent's state uniformly
+        [flight_path_angle, airspeed_norm, bank_angle] = np.random.uniform(self.observation_space.low, self.observation_space.high)
+        self.airplane.reset(flight_path_angle, airspeed_norm, bank_angle)
+
+        observation = self._get_obs(), {}
+
+        return observation
+
+    def step(self, action: list):
+        # Update state
+        action = np.clip(action, self.action_space.low, self.action_space.high)
+        c_lift = action[0]
+        bank_rate = action[1]
+
+        self.airplane.command_airplane(c_lift, bank_rate, 0)
+
+        # Calculate step reward: Height Loss
+        # TODO: Analyze policy performance based on reward implementation.
+        reward = self.airplane.TIME_STEP * self.airplane.airspeed_norm * np.sin(self.airplane.flight_path_angle)
+        # reward = self.TIME_STEP * (self.airspeed_norm * self.STALL_AIRSPEED) * np.sin(self.Flight Path) - 0.01 * bank_rate ** 2
+        terminated = self.termination()
+        observation = self._get_obs()
+        info = self._get_info()
+        return observation, reward, terminated, False, info
+
+
+    def termination(self,):
+        terminate =  np.where((self.airplane.flight_path_angle >= 0.0) & (self.airplane.airspeed_norm >= 1) , True, False)
+        return terminate

airplane/reduced_grumman.py

@@ -0,0 +1,56 @@
+import numpy as np
+from airplane.grumman import Grumman
+
+class ReducedGrumman(Grumman):
+    ####################################
+    ### Grumman American AA-1 Yankee ###
+    ####################################
+    """Class for simplified airplane state and dynamics"""
+
+    # NOTE: Commands as seperate objects? e.g. bank.rotate(airplane),
+    #       throttle.accelerate(airplane), etc.
+    # NOTE: Use of α instead of Cl?
+
+    def __init__(self):
+        super().__init__()
+        ##########################
+        ### Airplane variables ###
+        ##########################
+        self.flight_path_angle = np.zeros(10000, dtype=np.float32)                  # Flight Path Angle  (γ)  [rad]
+        self.airspeed_norm = np.ones_like(self.flight_path_angle, dtype=np.float32) # Air Speed  (V/Vs)  [1]
+        self.bank_angle = 0.0  # Bank Angle  (μ)  [rad]
+        # previous commands
+        self.last_c_lift = 0.0
+        self.last_bank_rate = 0.0
+        self.last_throttle = 0.0
+
+    def command_airplane(self, c_lift, bank_rate, delta_throttle):
+        self.last_c_lift = c_lift
+        self.last_bank_rate = bank_rate
+        self.last_throttle = delta_throttle
+
+        c_drag = self._cd_from_cl(c_lift)
+
+        # V_dot = - g sin γ - 0.5 * (ρ S V^2 CD / m) +  (thrust force / m) 
+        airspeed_dot = - self.GRAVITY * np.sin(self.flight_path_angle) - 0.5 * self.AIR_DENSITY * (
+                self.WING_SURFACE_AREA / self.MASS) * (self.airspeed_norm * self.STALL_AIRSPEED) ** 2 * c_drag \
+                       + (self.THROTTLE_LINEAR_MAPPING * delta_throttle / self.MASS)
+
+        # γ_dot = 0.5 * (ρ S V CL cos µ / m) - g cos γ / V
+        flight_path_angle_dot = 0.5 * self.AIR_DENSITY * (self.WING_SURFACE_AREA / self.MASS) * (
+                self.airspeed_norm * self.STALL_AIRSPEED) * c_lift * np.cos(self.bank_angle) \
+                              - (self.GRAVITY / (self.airspeed_norm * self.STALL_AIRSPEED)) * np.cos(
+            self.flight_path_angle)
+
+        # μ_dot = μ_dot_commanded
+        bank_angle_dot = bank_rate
+
+
+        self.airspeed_norm = self._update_state_from_derivative(self.airspeed_norm, airspeed_dot / self.STALL_AIRSPEED)
+        self.flight_path_angle = self._update_state_from_derivative(self.flight_path_angle, flight_path_angle_dot)
+        self.bank_angle = self._update_state_from_derivative(self.bank_angle, bank_angle_dot)
+
+    def reset(self, flight_path_angle, airspeed_norm, bank_angle):
+        self.flight_path_angle = flight_path_angle
+        self.airspeed_norm = airspeed_norm
+        self.bank_angle = bank_angle