Bug: Issue 115 static margin stability

rocketpy/Flight.py

@@ -1345,7 +1345,7 @@ def uDot(self, t, u, postProcessing=False):
             if aerodynamicSurface["name"] == "Fins":
                 Clfdelta, Cldomega, cantAngleRad = aerodynamicSurface["roll parameters"]
                 M3f = (
-                    (1 / 2 * rho * freestreamSpeed ** 2)
+                    (1 / 2 * rho * freestreamSpeed**2)
                     * self.rocket.area
                     * 2
                     * self.rocket.radius
@@ -1512,7 +1512,7 @@ def uDotParachute(self, t, u, postProcessing=False):
 
         return [vx, vy, vz, ax, ay, az, 0, 0, 0, 0, 0, 0, 0]
 
-    def postProcess(self):
+    def postProcess(self, interpolation="spline", extrapolation="natural"):
         """Post-process all Flight information produced during
         simulation. Includes the calculation of maximum values,
         calculation of secondary values such as energy and conversion
@@ -1530,43 +1530,43 @@ def postProcess(self):
         # Transform solution array into Functions
         sol = np.array(self.solution)
         self.x = Function(
-            sol[:, [0, 1]], "Time (s)", "X (m)", "spline", extrapolation="natural"
+            sol[:, [0, 1]], "Time (s)", "X (m)", interpolation, extrapolation
         )
         self.y = Function(
-            sol[:, [0, 2]], "Time (s)", "Y (m)", "spline", extrapolation="natural"
+            sol[:, [0, 2]], "Time (s)", "Y (m)", interpolation, extrapolation
         )
         self.z = Function(
-            sol[:, [0, 3]], "Time (s)", "Z (m)", "spline", extrapolation="natural"
+            sol[:, [0, 3]], "Time (s)", "Z (m)", interpolation, extrapolation
         )
         self.vx = Function(
-            sol[:, [0, 4]], "Time (s)", "Vx (m/s)", "spline", extrapolation="natural"
+            sol[:, [0, 4]], "Time (s)", "Vx (m/s)", interpolation, extrapolation
         )
         self.vy = Function(
-            sol[:, [0, 5]], "Time (s)", "Vy (m/s)", "spline", extrapolation="natural"
+            sol[:, [0, 5]], "Time (s)", "Vy (m/s)", interpolation, extrapolation
         )
         self.vz = Function(
-            sol[:, [0, 6]], "Time (s)", "Vz (m/s)", "spline", extrapolation="natural"
+            sol[:, [0, 6]], "Time (s)", "Vz (m/s)", interpolation, extrapolation
         )
         self.e0 = Function(
-            sol[:, [0, 7]], "Time (s)", "e0", "spline", extrapolation="natural"
+            sol[:, [0, 7]], "Time (s)", "e0", interpolation, extrapolation
         )
         self.e1 = Function(
-            sol[:, [0, 8]], "Time (s)", "e1", "spline", extrapolation="natural"
+            sol[:, [0, 8]], "Time (s)", "e1", interpolation, extrapolation
         )
         self.e2 = Function(
-            sol[:, [0, 9]], "Time (s)", "e2", "spline", extrapolation="natural"
+            sol[:, [0, 9]], "Time (s)", "e2", interpolation, extrapolation
         )
         self.e3 = Function(
-            sol[:, [0, 10]], "Time (s)", "e3", "spline", extrapolation="natural"
+            sol[:, [0, 10]], "Time (s)", "e3", interpolation, extrapolation
         )
         self.w1 = Function(
-            sol[:, [0, 11]], "Time (s)", "ω1 (rad/s)", "spline", extrapolation="natural"
+            sol[:, [0, 11]], "Time (s)", "ω1 (rad/s)", interpolation, extrapolation
         )
         self.w2 = Function(
-            sol[:, [0, 12]], "Time (s)", "ω2 (rad/s)", "spline", extrapolation="natural"
+            sol[:, [0, 12]], "Time (s)", "ω2 (rad/s)", interpolation, extrapolation
         )
         self.w3 = Function(
-            sol[:, [0, 13]], "Time (s)", "ω3 (rad/s)", "spline", extrapolation="natural"
+            sol[:, [0, 13]], "Time (s)", "ω3 (rad/s)", interpolation, extrapolation
         )
 
         # Process second type of outputs - accelerations
@@ -1598,12 +1598,12 @@ def postProcess(self):
                     self.alpha2.append([step[0], alpha2])
                     self.alpha3.append([step[0], alpha3])
         # Convert accelerations to functions
-        self.ax = Function(self.ax, "Time (s)", "Ax (m/s2)", "spline")
-        self.ay = Function(self.ay, "Time (s)", "Ay (m/s2)", "spline")
-        self.az = Function(self.az, "Time (s)", "Az (m/s2)", "spline")
-        self.alpha1 = Function(self.alpha1, "Time (s)", "α1 (rad/s2)", "spline")
-        self.alpha2 = Function(self.alpha2, "Time (s)", "α2 (rad/s2)", "spline")
-        self.alpha3 = Function(self.alpha3, "Time (s)", "α3 (rad/s2)", "spline")
+        self.ax = Function(self.ax, "Time (s)", "Ax (m/s2)", interpolation)
+        self.ay = Function(self.ay, "Time (s)", "Ay (m/s2)", interpolation)
+        self.az = Function(self.az, "Time (s)", "Az (m/s2)", interpolation)
+        self.alpha1 = Function(self.alpha1, "Time (s)", "α1 (rad/s2)", interpolation)
+        self.alpha2 = Function(self.alpha2, "Time (s)", "α2 (rad/s2)", interpolation)
+        self.alpha3 = Function(self.alpha3, "Time (s)", "α3 (rad/s2)", interpolation)
 
         # Process third type of outputs - temporary values calculated during integration
         # Initialize force and atmospheric arrays
@@ -1630,25 +1630,35 @@ def postProcess(self):
                     # Call derivatives in post processing mode
                     uDot = currentDerivative(step[0], step[1:], postProcessing=True)
         # Convert forces and atmospheric arrays to functions
-        self.R1 = Function(self.R1, "Time (s)", "R1 (N)", "spline")
-        self.R2 = Function(self.R2, "Time (s)", "R2 (N)", "spline")
-        self.R3 = Function(self.R3, "Time (s)", "R3 (N)", "spline")
-        self.M1 = Function(self.M1, "Time (s)", "M1 (Nm)", "spline")
-        self.M2 = Function(self.M2, "Time (s)", "M2 (Nm)", "spline")
-        self.M3 = Function(self.M3, "Time (s)", "M3 (Nm)", "spline")
+        self.R1 = Function(self.R1, "Time (s)", "R1 (N)", interpolation)
+        self.R2 = Function(self.R2, "Time (s)", "R2 (N)", interpolation)
+        self.R3 = Function(self.R3, "Time (s)", "R3 (N)", interpolation)
+        self.M1 = Function(self.M1, "Time (s)", "M1 (Nm)", interpolation)
+        self.M2 = Function(self.M2, "Time (s)", "M2 (Nm)", interpolation)
+        self.M3 = Function(self.M3, "Time (s)", "M3 (Nm)", interpolation)
         self.windVelocityX = Function(
-            self.windVelocityX, "Time (s)", "Wind Velocity X (East) (m/s)", "spline"
+            self.windVelocityX,
+            "Time (s)",
+            "Wind Velocity X (East) (m/s)",
+            interpolation,
         )
         self.windVelocityY = Function(
-            self.windVelocityY, "Time (s)", "Wind Velocity Y (North) (m/s)", "spline"
+            self.windVelocityY,
+            "Time (s)",
+            "Wind Velocity Y (North) (m/s)",
+            interpolation,
+        )
+        self.density = Function(
+            self.density, "Time (s)", "Density (kg/m³)", interpolation
+        )
+        self.pressure = Function(
+            self.pressure, "Time (s)", "Pressure (Pa)", interpolation
         )
-        self.density = Function(self.density, "Time (s)", "Density (kg/m³)", "spline")
-        self.pressure = Function(self.pressure, "Time (s)", "Pressure (Pa)", "spline")
         self.dynamicViscosity = Function(
-            self.dynamicViscosity, "Time (s)", "Dynamic Viscosity (Pa s)", "spline"
+            self.dynamicViscosity, "Time (s)", "Dynamic Viscosity (Pa s)", interpolation
         )
         self.speedOfSound = Function(
-            self.speedOfSound, "Time (s)", "Speed of Sound (m/s)", "spline"
+            self.speedOfSound, "Time (s)", "Speed of Sound (m/s)", interpolation
         )
 
         # Process fourth type of output - values calculated from previous outputs

rocketpy/Rocket.py

@@ -591,36 +591,36 @@ def addFins(
             (s / 3) * (Cr + 2 * Ct) / Yr
         )  # span wise position of fin's mean aerodynamic chord
         gamac = np.arctan((Cr - Ct) / (2 * span))
-        Lf = np.sqrt((rootChord / 2 - tipChord / 2) ** 2 + span ** 2)
+        Lf = np.sqrt((rootChord / 2 - tipChord / 2) ** 2 + span**2)
         radius = self.radius if radius == 0 else radius
         d = 2 * radius
-        Aref = np.pi * radius ** 2
-        AR = 2 * s ** 2 / Af  # Barrowman's convention for fin's aspect ratio
+        Aref = np.pi * radius**2
+        AR = 2 * s**2 / Af  # Barrowman's convention for fin's aspect ratio
         cantAngleRad = np.radians(cantAngle)
         trapezoidalConstant = (
-            (Cr + 3 * Ct) * s ** 3
-            + 4 * (Cr + 2 * Ct) * radius * s ** 2
-            + 6 * (Cr + Ct) * s * radius ** 2
+            (Cr + 3 * Ct) * s**3
+            + 4 * (Cr + 2 * Ct) * radius * s**2
+            + 6 * (Cr + Ct) * s * radius**2
         ) / 12
 
         # Fin–body interference correction parameters
         τ = (s + radius) / radius
         λ = Ct / Cr
         liftInterferenceFactor = 1 + 1 / τ
-        rollForcingInterferenceFactor = (1 / np.pi ** 2) * (
-            (np.pi ** 2 / 4) * ((τ + 1) ** 2 / τ ** 2)
-            + ((np.pi * (τ ** 2 + 1) ** 2) / (τ ** 2 * (τ - 1) ** 2))
-            * np.arcsin((τ ** 2 - 1) / (τ ** 2 + 1))
+        rollForcingInterferenceFactor = (1 / np.pi**2) * (
+            (np.pi**2 / 4) * ((τ + 1) ** 2 / τ**2)
+            + ((np.pi * (τ**2 + 1) ** 2) / (τ**2 * (τ - 1) ** 2))
+            * np.arcsin((τ**2 - 1) / (τ**2 + 1))
             - (2 * np.pi * (τ + 1)) / (τ * (τ - 1))
-            + ((τ ** 2 + 1) ** 2)
-            / (τ ** 2 * (τ - 1) ** 2)
-            * (np.arcsin((τ ** 2 - 1) / (τ ** 2 + 1))) ** 2
-            - (4 * (τ + 1)) / (τ * (τ - 1)) * np.arcsin((τ ** 2 - 1) / (τ ** 2 + 1))
-            + (8 / (τ - 1) ** 2) * np.log((τ ** 2 + 1) / (2 * τ))
+            + ((τ**2 + 1) ** 2)
+            / (τ**2 * (τ - 1) ** 2)
+            * (np.arcsin((τ**2 - 1) / (τ**2 + 1))) ** 2
+            - (4 * (τ + 1)) / (τ * (τ - 1)) * np.arcsin((τ**2 - 1) / (τ**2 + 1))
+            + (8 / (τ - 1) ** 2) * np.log((τ**2 + 1) / (2 * τ))
         )
         rollDampingInterferenceFactor = 1 + (
             ((τ - λ) / (τ)) - ((1 - λ) / (τ - 1)) * np.log(τ)
-        ) / (((τ + 1) * (τ - λ)) / (2) - ((1 - λ) * (τ ** 3 - 1)) / (3 * (τ - 1)))
+        ) / (((τ + 1) * (τ - λ)) / (2) - ((1 - λ) * (τ**3 - 1)) / (3 * (τ - 1)))
 
         # Save geometric parameters for later Fin Flutter Analysis and Roll Moment Calculation
         self.rootChord = Cr
@@ -648,11 +648,11 @@ def beta(mach):
             """
 
             if mach < 0.8:
-                return np.sqrt(1 - mach ** 2)
+                return np.sqrt(1 - mach**2)
             elif mach < 1.1:
-                return np.sqrt(1 - 0.8 ** 2)
+                return np.sqrt(1 - 0.8**2)
             else:
-                return np.sqrt(mach ** 2 - 1)
+                return np.sqrt(mach**2 - 1)
 
         # Defines number of fins correction
         def finNumCorrection(n):
@@ -770,7 +770,7 @@ def cnalfa1(cn):
             * clalphaSingleFin
             * np.cos(cantAngleRad)
             * trapezoidalConstant
-            / (Aref * d ** 2)
+            / (Aref * d**2)
         )
         # Function of mach number
         rollParameters = [clfDelta, cldOmega, cantAngleRad]

tests/test_flight.py

@@ -1,9 +1,56 @@
 import datetime
 from unittest.mock import patch
 
+import numpy as np
 import pytest
+from rocketpy import Environment, Flight, Rocket, SolidMotor
+from scipy import optimize
+
+# Helper functions
+def setup_rocket_with_given_static_margin(rocket, static_margin):
+    """Takes any rocket, removes its aerodynamic surfaces and adds a set of
+    nose, fins and tail specially designed to have a given static margin.
+    The rocket is modified in place.
+
+    Parameters
+    ----------
+    rocket : Rocket
+        Rocket to be modified
+    static_margin : float
+        Static margin that the given rocket shall have
+
+    Returns
+    -------
+    rocket : Rocket
+        Rocket with the given static margin.
+    """
+
+    def compute_static_margin_error_given_distance(distanceToCM, static_margin, rocket):
+        rocket.aerodynamicSurfaces = []
+        rocket.addNose(length=0.5, kind="vonKarman", distanceToCM=1.0)
+        rocket.addFins(
+            4,
+            span=0.100,
+            rootChord=0.100,
+            tipChord=0.100,
+            distanceToCM=distanceToCM,
+        )
+        rocket.addTail(
+            topRadius=0.0635,
+            bottomRadius=0.0435,
+            length=0.060,
+            distanceToCM=-1.194656,
+        )
+        return rocket.staticMargin(0) - static_margin
+
+    sol = optimize.root_scalar(
+        compute_static_margin_error_given_distance,
+        bracket=[-2.0, 2.0],
+        method="brentq",
+        args=(static_margin, rocket),
+    )
 
-from rocketpy import Environment, SolidMotor, Rocket, Flight
+    return rocket
 
 
 @patch("matplotlib.pyplot.show")
@@ -209,6 +256,86 @@ def mainTrigger(p, y):
     assert test_flight.allInfo() == None
 
 
+@pytest.mark.parametrize("wind_u, wind_v", [(0, 10), (0, -10), (10, 0), (-10, 0)])
+@pytest.mark.parametrize(
+    "static_margin, max_time",
+    [(-0.1, 2), (-0.01, 5), (0, 5), (0.01, 20), (0.1, 20), (1.0, 20)],
+)
+def test_stability_static_margins(wind_u, wind_v, static_margin, max_time):
+    """Test stability margins for a constant velocity flight, 100 m/s, wind a
+    lateral wind speed of 10 m/s. Rocket has infinite mass to prevent side motion.
+    Check if a restoring moment exists depending on static margins."""
+
+    # Create an environment with ZERO gravity to keep the rocket's speed constant
+    Env = Environment(gravity=0, railLength=0, latitude=0, longitude=0, elevation=0)
+    Env.setAtmosphericModel(
+        type="CustomAtmosphere",
+        wind_u=wind_u,
+        wind_v=wind_v,
+        pressure=101325,
+        temperature=300,
+    )
+
+    # Create a motor with ZERO thrust and ZERO mass to keep the rocket's speed constant
+    DummyMotor = SolidMotor(
+        thrustSource=1e-300,
+        burnOut=1e-10,
+        grainNumber=5,
+        grainSeparation=5 / 1000,
+        grainDensity=1e-300,
+        grainOuterRadius=33 / 1000,
+        grainInitialInnerRadius=15 / 1000,
+        grainInitialHeight=120 / 1000,
+        nozzleRadius=33 / 1000,
+        throatRadius=11 / 1000,
+    )
+
+    # Create a rocket with ZERO drag and HUGE mass to keep the rocket's speed constant
+    DummyRocket = Rocket(
+        motor=DummyMotor,
+        radius=127 / 2000,
+        mass=100e3,
+        inertiaI=1,
+        inertiaZ=0.0351,
+        distanceRocketNozzle=-1.255,
+        distanceRocketPropellant=-0.85704,
+        powerOffDrag=0,
+        powerOnDrag=0,
+    )
+    DummyRocket.setRailButtons([0.2, -0.5])
+    setup_rocket_with_given_static_margin(DummyRocket, static_margin)
+
+    # Simulate
+    init_pos = [0, 0, 100]  # Start at 100 m of altitude
+    init_vel = [0, 0, 100]  # Start at 100 m/s
+    init_att = [1, 0, 0, 0]  # Inclination of 90 deg and heading of 0 deg
+    init_angvel = [0, 0, 0]
+    initial_solution = [0] + init_pos + init_vel + init_att + init_angvel
+    TestFlight = Flight(
+        rocket=DummyRocket,
+        environment=Env,
+        initialSolution=initial_solution,
+        maxTime=max_time,
+        maxTimeStep=1e-2,
+        verbose=False,
+    )
+    TestFlight.postProcess()
+
+    # Check stability according to static margin
+    if wind_u == 0:
+        moments = TestFlight.M1.source[:, 1]
+        wind_sign = np.sign(wind_v)
+    else:  # wind_v == 0
+        moments = TestFlight.M2.source[:, 1]
+        wind_sign = -np.sign(wind_u)
+
+    assert (
+        (static_margin > 0 and np.max(moments) * np.min(moments) < 0)
+        or (static_margin < 0 and np.all(moments / wind_sign <= 0))
+        or (static_margin == 0 and np.all(np.abs(moments) <= 1e-10))
+    )
+
+
 @patch("matplotlib.pyplot.show")
 def test_rolling_flight(mock_show):
     test_env = Environment(