EquationGainScheduling

MAPLEAF/Examples/Simulations/Canards.mapleaf

@@ -27,7 +27,7 @@ Rocket{
         desiredFlightDirection      (0 0 1) # Define flight direction to reach/stabilize, in launch tower frame
 
         MomentController{
-            Type                    ScheduledGainPIDRocket # Only option - expects one set of coefficients for longitudinal PID controller and one set for roll PID controller
+            Type                    TableScheduledGainPIDRocket # Only option - expects one set of coefficients for longitudinal PID controller and one set for roll PID controller
             gainTableFilePath       MAPLEAF/Examples/TabulatedData/constPIDCoeffs.txt
             scheduledBy             Mach Altitude # Mach, Altitude, UnitReynolds, AOA, RollAngle - order must match table
         }

MAPLEAF/Examples/Simulations/EquationGainScheduled.mapleaf

@@ -0,0 +1,163 @@
+# MAPLEAF
+# See SimDefinitionTemplate.mapleaf for file format info & description of all options
+
+SimControl{
+    timeDiscretization RK45Adaptive
+    timeStep 0.02 #sec, CanardDeflections
+    plot Position Velocity AngularVelocity Deflection&canardsFin FlightAnimation
+    loggingLevel 2
+
+    EndCondition Apogee
+    EndConditionValue 0
+
+    TimeStepAdaptation{
+        controller PID
+    }
+}
+
+Rocket{             
+
+    # Initial state
+    position            (0 0 10) # m
+    initialDirection    (0 0.1 1)
+    velocity            (0 0 10) #m/s
+
+
+    ControlSystem{
+        desiredFlightDirection      (0 0 1) # Define flight direction to reach/stabilize, in launch tower frame
+
+        MomentController{
+            Type                            EquationScheduledGainPIDRocket # Only option - expects one set of coefficients for longitudinal PID controller and one set for roll PID controller
+            lateralGainCoeffFilePath        MAPLEAF/Examples/TabulatedData/lateralPIDEquationCoeffs.txt
+            longitudinalGainCoeffFilePath   MAPLEAF/Examples/TabulatedData/longitudinalPIDEquationCoeffs.txt
+            scheduledBy                     Mach Altitude # Mach, Altitude, UnitReynolds, AOA, RollAngle - must match Aeroparameters
+            equationOrder                   2
+        }
+
+        # Simulation will not take time steps larger than 1/updateRate
+            # If an update rate is specified and adaptive time stepping is selected, adaptive time stepping will only be used during the descent/recovery portion of the flight
+                # Constant RK4 time stepping will be substituted for the ascent portion
+                    # Specified initial time step will be rounded to the nearest integer divisor of the control system time step
+            # With an updateRate of 0 (default), the control system will simply run once per time step
+                # Note that because control system updates happen between Runge-Kutta time steps,
+                    # errors predicted/estimated by the adaptive time stepping methods will not include errors due to low control system update rates.
+        updateRate                  100 # Hz
+
+        controlledSystem            Rocket.Sustainer.Canards # Enter path to the controlled component in the Rocket
+    }
+
+    Sustainer{
+        class           Stage
+        stageNumber      0 #First and only stage
+
+        # Constant mass properties - remove to use component-buildup mass/cg/MOI
+        constCG             (0 0 -2.65) #m
+        constMass           50 # kg
+        constMOI            (85 85 0.5) # kg*m^2
+
+        Nosecone{
+            class           Nosecone
+            mass            20.0
+            position        (0 0 0)
+            cg              (0 0 -0.2)
+            baseDiameter    0.1524
+            aspectRatio     5
+            shape           tangentOgive
+
+            surfaceRoughness    0.000050
+        }
+
+        UpperBodyTube{
+            class           Bodytube
+            mass            5 
+            position        (0 0 -0.762)
+            cg              (0 0 -1)
+            outerDiameter   0.1524
+            length          3.81
+
+            surfaceRoughness    0.000050
+        }
+
+        Canards{
+            class           FinSet
+
+            mass            2 # kg
+            position        (0 0 -0.8636)
+            cg              (0 0 -0.8636)
+
+            numFins         4
+            sweepAngle      30 # deg
+            rootChord       0.1524 # m
+            tipChord        0.0762 # m
+            span            0.0635 # m
+
+            thickness       0.0047625 # m
+            surfaceRoughness    0.000050
+
+            Actuators{
+                class                   Actuator
+                controller              TableInterpolating
+
+                deflectionTablePath     MAPLEAF/Examples/TabulatedData/linearCanardDefls.txt
+
+                # Mach, Altitude, UnitReynolds, AOA, RollAngle, DesiredMx, DesiredMy, DesiredMz - order must match the order of the key columns in table
+                    # Desired moments must come last
+                deflectionKeyColumns    Mach Altitude DesiredMx DesiredMy DesiredMz
+
+                minDeflection           -45
+                maxDeflection           45                
+
+                responseModel           FirstOrder # Only Choice
+                responseTime            0.1 # seconds
+            }
+        }
+
+        GeneralMass{
+            class           Mass
+            mass            5
+            position        (0 0 -2.762)
+            cg              (0 0 -2.762)
+        }
+
+        Motor{
+            class           Motor
+            path            MAPLEAF/Examples/Motors/test2.txt
+        }
+
+        TailFins{
+            class           FinSet 
+            mass            2 # kg
+            position        (0 0 -4.2672)
+            cg              (0 0 -4.2762)
+
+            numFins         4
+            sweepAngle      28.61 # deg
+            rootChord       0.3048 # m
+            tipChord        0.1524 # m
+            span            0.1397 # m
+            thickness       0.0047625 # m
+            surfaceRoughness    0.000050
+        }
+
+        RecoverySystem{
+            class           RecoverySystem
+            mass            0
+            position        (0 0 -1)
+            cg              (0 0 -1)
+            numStages       2
+
+            # Apogee, Time, Altitude
+            stage1Trigger       Apogee
+            stage1TriggerValue  30 # sec from launch (Time), m AGL, reached while descending (Altitude), unneeded for Apogee
+            stage1ChuteArea     2 # m^2
+            stage1Cd            1.5 # Drag Coefficient (~0.75-0.8 for flat sheet, 1.5-1.75 for domed chute)
+            stage1DelayTime     2 #s
+
+            stage2Trigger       Altitude
+            stage2TriggerValue  300 # sec from launch (Time), m AGL, reached while descending (Altitude), unneeded for Apogee
+            stage2ChuteArea     9 # m^2
+            stage2Cd            1.5 # Drag Coefficient (~0.75-0.8 for flat sheet, 1.5-1.75 for domed chute)
+            stage2DelayTime     0 #s
+        }
+    }
+}

MAPLEAF/Examples/TabulatedData/lateralPIDEquationCoeffs.txt

@@ -0,0 +1,7 @@
+P,I,D
+1,1,1
+2,2,2
+3,3,3
+4,4,4
+5,5,5
+6,6,6

MAPLEAF/Examples/TabulatedData/longitudinalPIDEquationCoeffs.txt

@@ -0,0 +1,7 @@
+P,I,D
+1,1,1
+2,2,2
+3,3,3
+4,4,4
+5,5,5
+6,6,6

MAPLEAF/GNC/ControlSystems.py

@@ -9,7 +9,9 @@
 import numpy as np
 from MAPLEAF.GNC import (ConstantGainPIDRocketMomentController,
                          IdealMomentController,
-                         ScheduledGainPIDRocketMomentController, Stabilizer)
+                         TableScheduledGainPIDRocketMomentController, 
+                         EquationScheduledGainPIDRocketMomentController,
+                         Stabilizer)
 from MAPLEAF.IO import Log, SubDictReader
 from MAPLEAF.Motion import integratorFactory
 
@@ -66,14 +68,20 @@ def __init__(self, controlSystemDictReader, rocket, initTime=0, log=False, silen
             Iz = controlSystemDictReader.getFloat("MomentController.Iz")
             Dz = controlSystemDictReader.getFloat("MomentController.Dz")
             self.momentController = ConstantGainPIDRocketMomentController(Pxy,Ixy,Dxy,Pz,Iz,Dz)
-        elif momentControllerType == "ScheduledGainPIDRocket":
+        elif momentControllerType == "TableScheduledGainPIDRocket":
             gainTableFilePath = controlSystemDictReader.getString("MomentController.gainTableFilePath")
             keyColumnNames = controlSystemDictReader.getString("MomentController.scheduledBy").split()
-            self.momentController = ScheduledGainPIDRocketMomentController(gainTableFilePath, keyColumnNames)
+            self.momentController = TableScheduledGainPIDRocketMomentController(gainTableFilePath, keyColumnNames)
+        elif momentControllerType == "EquationScheduledGainPIDRocket":
+            lateralGainCoeffFilePath = controlSystemDictReader.getString("MomentController.lateralGainCoeffFilePath")
+            longitudinalGainCoeffFilePath = controlSystemDictReader.getString("MomentController.longitudinalGainCoeffFilePath")
+            parameterList = controlSystemDictReader.getString("MomentController.scheduledBy").split()
+            equationOrder = controlSystemDictReader.getInt("MomentController.equationOrder")
+            self.momentController = EquationScheduledGainPIDRocketMomentController(lateralGainCoeffFilePath, longitudinalGainCoeffFilePath, parameterList, equationOrder, controlSystemDictReader)
         elif momentControllerType == "IdealMomentController":
             self.momentController = IdealMomentController(self.rocket)
         else:
-            raise ValueError("Moment Controller Type: {} not implemented. Try ScheduledGainPIDRocket or IdealMomentController".format(momentControllerType))
+            raise ValueError("Moment Controller Type: {} not implemented. Try TableScheduledGainPIDRocket or IdealMomentController".format(momentControllerType))
 
         ### Set update rate ###
         if momentControllerType == "IdealMomentController":

MAPLEAF/GNC/MomentControllers.py

@@ -5,18 +5,21 @@
 import abc
 
 import numpy as np
+import pandas as pd
+
+from itertools import combinations_with_replacement as cwithr
 
 from MAPLEAF.Motion import AeroParameters, AngularVelocity, Vector
-from MAPLEAF.GNC import ConstantGainPIDController, ScheduledGainPIDController
+from MAPLEAF.GNC import *
 
-__all__ = ["ConstantGainPIDRocketMomentController", "ScheduledGainPIDRocketMomentController", "MomentController", "IdealMomentController" ]
+__all__ = ["ConstantGainPIDRocketMomentController", "TableScheduledGainPIDRocketMomentController", "EquationScheduledGainPIDRocketMomentController", "MomentController", "IdealMomentController" ]
 
 class MomentController(abc.ABC):
     @abc.abstractmethod
     def getDesiredMoments(self, rocketState, environment, targetOrientation, time, dt):
         ''' Should return a list [ desired x-axis, y-axis, and z-axis ] moments '''
 
-class ScheduledGainPIDRocketMomentController(MomentController, ScheduledGainPIDController):
+class TableScheduledGainPIDRocketMomentController(MomentController, TableScheduledGainPIDController):
     def __init__(self, gainTableFilePath, keyColumnNames):
         '''
             Assumes the longitudinal (Pitch/Yaw) PID coefficients are in columns nKeyColumns:nKeyColumns+2
@@ -25,7 +28,7 @@ def __init__(self, gainTableFilePath, keyColumnNames):
         '''
         self.keyFunctionList = [ AeroParameters.stringToAeroFunctionMap[x] for x in keyColumnNames ]
         nKeyColumns = len(keyColumnNames)
-        ScheduledGainPIDController.__init__(self, gainTableFilePath, nKeyColumns, PCol=nKeyColumns, DCol=nKeyColumns+5)
+        TableScheduledGainPIDController.__init__(self, gainTableFilePath, nKeyColumns, PCol=nKeyColumns, DCol=nKeyColumns+5)
 
     def updateCoefficientsFromGainTable(self, keyList):
         ''' Overriding parent class method to enable separate longitudinal and roll coefficients in a single controller '''
@@ -55,6 +58,72 @@ def getDesiredMoments(self, rocketState, environment, targetOrientation, time, d
         self.updateCoefficientsFromGainTable(gainKeyList)
         return self.getNewSetPoint(orientationError, dt)
 
+class EquationScheduledGainPIDRocketMomentController(MomentController):
+
+    def __init__(self, lateralCoefficientsPath, longitudinalCoefficientsPath, parameterList, equationOrder, controlSystemDictReader):
+
+      def _getEquationCoefficientsFromTextFile(textFilePath):
+        equationCoefficients = pd.read_csv(textFilePath)
+        fileHeader = equationCoefficients.columns.to_list()
+
+        if fileHeader != ['P','I','D']:
+          raise ValueError("The data in text file {} is not in the proper format".format(textFilePath))
+
+        PCoefficients = equationCoefficients["P"].to_list()
+        ICoefficients = equationCoefficients["I"].to_list()
+        DCoefficients = equationCoefficients["D"].to_list()
+
+        allCoefficients = []
+        allCoefficients.append(PCoefficients)
+        allCoefficients.append(ICoefficients)
+        allCoefficients.append(DCoefficients)
+
+        return allCoefficients
+      #parameterList must contains strings that match those in Motion/AeroParameters
+      self.parameterFetchFunctionList = [ AeroParameters.stringToAeroFunctionMap[x] for x in parameterList ]
+
+      pitchCoefficientList = _getEquationCoefficientsFromTextFile(lateralCoefficientsPath)
+      yawCoefficientList = pitchCoefficientList
+      rollCoefficientList = _getEquationCoefficientsFromTextFile(longitudinalCoefficientsPath)
+
+      for i in range(len(pitchCoefficientList)):
+        controlSystemDictReader.simDefinition.setValue('PitchC' + str(i),pitchCoefficientList[i])
+        controlSystemDictReader.simDefinition.setValue('YawC' + str(i),yawCoefficientList[i])
+        controlSystemDictReader.simDefinition.setValue('RollC' + str(i),rollCoefficientList[i])
+
+
+      self.pitchController = EquationScheduledGainPIDController(pitchCoefficientList, parameterList, equationOrder)
+      self.yawController = EquationScheduledGainPIDController(yawCoefficientList, parameterList, equationOrder)
+      self.rollController = EquationScheduledGainPIDController(rollCoefficientList, parameterList, equationOrder)
+
+    def _getOrientationError(self, rocketState, targetOrientation):
+        return np.array((targetOrientation / rocketState.orientation).toRotationVector())
+
+    def getDesiredMoments(self, rocketState, environment, targetOrientation, time, dt):
+
+      orientationError = self._getOrientationError(rocketState, targetOrientation)
+      variableFunctionList= AeroParameters.getAeroPropertiesList(self.parameterFetchFunctionList, rocketState, environment)
+      self._updateCoefficientsFromEquation(variableFunctionList)
+
+      return self._getNewSetPoint(orientationError, dt)
+
+    def _updateCoefficientsFromEquation(self, variableFunctionList):
+
+      self.yawController.updateCoefficientsFromEquation(variableFunctionList)
+      self.pitchController.updateCoefficientsFromEquation(variableFunctionList)
+      self.rollController.updateCoefficientsFromEquation(variableFunctionList)
+
+    def _getNewSetPoint(self,currentError,dt):
+
+
+      output = [0,0,0]
+      output[0] = self.pitchController.getNewSetPoint(currentError[0],dt)
+      output[1] = self.yawController.getNewSetPoint(currentError[1],dt)
+      output[2] = self.rollController.getNewSetPoint(currentError[2],dt)
+
+      return output
+
+
 class ConstantGainPIDRocketMomentController(MomentController, ConstantGainPIDController):
     def __init__(self, Pxy, Ixy, Dxy, Pz, Iz, Dz):
         '''