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):
         '''

MAPLEAF/GNC/PID.py

@@ -1,9 +1,11 @@
 ''' PID controllers control parts of the control system and adaptive simulation timestepping '''
 
+from itertools import combinations_with_replacement as cwithr
+
 import numpy as np
 from MAPLEAF.Motion import NoNaNLinearNDInterpolator
 
-__all__ = [ "PIDController", "ConstantGainPIDController", "ScheduledGainPIDController" ]
+__all__ = [ "PIDController", "ConstantGainPIDController", "TableScheduledGainPIDController", "EquationScheduledGainPIDController"]
 
 class PIDController():
 
@@ -59,7 +61,25 @@ def updateCoefficients(self, P, I, D, maxIntegral=None):
     def resetIntegral(self):
         self.errorIntegral = self.lastError * 0 # Done to handle arbitrary size np arrays
 
-class ScheduledGainPIDController(PIDController):
+class ConstantGainPIDController(PIDController):
+
+    def __init__(self, P=0, I=0, D=0, initialError=0, maxIntegral=None):
+        '''
+            Inputs:
+                P:                  (int) Proportional Gain
+                I:                  (int) Integral Gain
+                D:                  (int) Derivative Gain
+                DCol:               (int) zero-indexed column number of D Coefficient
+
+                Note:
+                    It is assumed that PCol, ICol, and DCol exist one after another in the table
+                
+                Inputs passed through to parent class (PICController):
+                    initialError, maxIntegral
+        '''
+        PIDController.__init__(self, P,I,D, initialError=initialError, maxIntegral=maxIntegral)
+
+class TableScheduledGainPIDController(PIDController):
     def __init__(self, gainTableFilePath, nKeyColumns=2, PCol=3, DCol=5, initialError=0, maxIntegral=None):
         '''
             Inputs:
@@ -88,20 +108,80 @@ def updateCoefficientsFromGainTable(self, keyList):
         P, I, D = self._getPIDCoeffs(keyList)
         self.updateCoefficients(P, I, D)
 
-class ConstantGainPIDController(PIDController):
-
-    def __init__(self, P=0, I=0, D=0, initialError=0, maxIntegral=None):
+class EquationScheduledGainPIDController(PIDController):
+    def __init__(self, coefficientMatrix, parameterList, equationOrder, initialError=0, maxIntegral=None):
         '''
             Inputs:
-                P:                  (int) Proportional Gain
-                I:                  (int) Integral Gain
-                D:                  (int) Derivative Gain
-                DCol:               (int) zero-indexed column number of D Coefficient
-
-                Note:
-                    It is assumed that PCol, ICol, and DCol exist one after another in the table
+                coefficientList     (int) List of coefficients to be used in the gain scheduling equation
+                parameterList:      (string) List of names of the parameters used in the gain scheduling, must be in the standardParameters dictionary
+                equationOrder:       (int) Max order of the gain schedule equation
                 
-                Inputs passed through to parent class (PICController):
+                Inputs passed through to parent class (PIDController):
                     initialError, maxIntegral
         '''
-        PIDController.__init__(self, P,I,D, initialError=initialError, maxIntegral=maxIntegral)
+        PIDController.__init__(self, 0,0,0, initialError=initialError, maxIntegral=maxIntegral)
+
+        #Move inputs into internal variables
+        self.equationOrder = equationOrder
+        self.PcoefficientList = coefficientMatrix[0]
+        self.IcoefficientList = coefficientMatrix[1]
+        self.DcoefficientList = coefficientMatrix[2]
+        self.numberedVariableList = []
+        numVariables = 0
+
+        #Create a list that represents the parameters as numbers
+        self.numberedParameterList = []
+        for i in range(len(parameterList)):
+          self.numberedParameterList.append(i)
+
+        #variablesList is a list containing every variable combination using the equation order and the parameter list
+        #numVariables is used to check that correct number of coefficients was provided
+        for i in range(self.equationOrder+1):
+          parameterCombinationsList = list(cwithr(self.numberedParameterList, i))
+          numVariables = numVariables + len(parameterCombinationsList)
+          self.numberedVariableList.append(parameterCombinationsList)
+
+        #Store the number of coefficients
+        self.numPCoefficients = len(self.PcoefficientList)
+        self.numICoefficients = len(self.IcoefficientList)
+        self.numDCoefficients = len(self.DcoefficientList)
+
+        if self.numPCoefficients != numVariables:
+            raise ValueError("Number of given P coefficients: {}, not suitable for equation of order {} with {} scheduled parameters".format(len(self.PcoefficientList),\
+            self.equationOrder,len(parameterList)))
+
+        if self.numICoefficients != numVariables:
+            raise ValueError("Number of given I coefficients: {}, not suitable for equation of order {} with {} scheduled parameters".format(len(self.IcoefficientList),\
+            self.equationOrder,len(parameterList)))
+
+        if self.numDCoefficients != numVariables:
+            raise ValueError("Number of given D coefficients: {}, not suitable for equation of order {} with {} scheduled parameters".format(len(self.DcoefficientList),\
+            self.equationOrder,len(parameterList)))
+
+        self.variableValuesList = np.zeros(numVariables)
+
+    def _updateVariableValuesFromParameters(self,parameterValueList):
+
+        variableIndex = 0
+        for i in range(len(self.numberedVariableList)):
+          for j in range(len(self.numberedVariableList[i])):
+            variableValue = 1
+            if i != 0: #Skipping the empty "constant"entry for now"
+                for k in range(len(self.numberedVariableList[i][j])):
+                  temp = parameterValueList[self.numberedVariableList[i][j][k]]
+                  variableValue = variableValue*temp
+            self.variableValuesList[variableIndex] = variableValue
+            variableIndex = variableIndex + 1
+
+    def updateCoefficientsFromEquation(self,parameterValueList):
+
+      self._updateVariableValuesFromParameters(parameterValueList)
+      P = 0
+      I = 0
+      D = 0
+      for i in range(len(self.variableValuesList)):
+        P = P + self.variableValuesList[i]*self.PcoefficientList[i]
+        I = I + self.variableValuesList[i]*self.IcoefficientList[i]
+        D = D + self.variableValuesList[i]*self.DcoefficientList[i]
+
+      self.updateCoefficients(P,I,D)

MAPLEAF/IO/subDictReader.py

@@ -119,3 +119,4 @@ def getImmediateSubKeys(self, key=None) -> List[str]:
     def getDictName(self) -> str:
         lastDotIndex = self.simDefDictPathToReadFrom.rfind('.')
         return self.simDefDictPathToReadFrom[lastDotIndex+1:]
+

MAPLEAF/SimulationRunners/Batch.py

@@ -48,8 +48,8 @@ def __init__(self,
             printStackTraces=False, 
             include=None, 
             exclude=None,
-            percentErrorTolerance=0.1,
-            absoluteErrorTolerance=1e-10,
+            percentErrorTolerance=0.2,
+            absoluteErrorTolerance=2e-10,
             resultToValidate=None
         ):
         self.batchDefinition = batchDefinition

MAPLEAF/SimulationRunners/SingleSimulations.py

@@ -356,16 +356,36 @@ def _logSimulationResults(self, simDefinition):
 
             # Create a new folder for the results of the current simulation
             periodIndex = simDefinition.fileName.rfind('.')
-            resultsFolderName = simDefinition.fileName[:periodIndex] + "_Run"
-            resultsFolderName = Logging.findNextAvailableNumberedFileName(fileBaseName=resultsFolderName, extension="")
-            os.mkdir(resultsFolderName)
+            resultsFolderBaseName = simDefinition.fileName[:periodIndex] + "_Run"
+
+            def tryCreateResultsFolder(resultsFolderBaseName):
+                resultsFolderName = Logging.findNextAvailableNumberedFileName(fileBaseName=resultsFolderBaseName, extension="")
+
+                try:
+                    os.mkdir(resultsFolderName)
+                    return resultsFolderName
+
+                except FileExistsError:
+                    # End up here if another process created the same results folder 
+                        # (other thread runs os.mkdir b/w when this thread runs findNextAvailableNumberedFileName and os.mkdir)
+                        # Should only happen during parallel runs
+                    return ""
+
+            createdResultsFolder = tryCreateResultsFolder(resultsFolderBaseName)
+            iterations = 0
+            while createdResultsFolder == "" and iterations < 50:
+                createdResultsFolder = tryCreateResultsFolder(resultsFolderBaseName)
+                iterations += 1
+
+            if iterations == 50:
+                raise ValueError("Repeated error (50x): unable to create a results folder: {}.".format(resultsFolderBaseName))
 
             # Write logs to file
             for rocket in self.rocketStages:
-                logFilePaths += rocket.writeLogsToFile(resultsFolderName)            
+                logFilePaths += rocket.writeLogsToFile(createdResultsFolder)            
 
             # Output console output
-            consoleOutputPath = os.path.join(resultsFolderName, "consoleOutput.txt")
+            consoleOutputPath = os.path.join(createdResultsFolder, "consoleOutput.txt")
             print("Writing log file: {}".format(consoleOutputPath))
             with open(consoleOutputPath, 'w+') as file:
                 file.writelines(self.consoleOutputLog)

test/test_GNC/test_MomentControllers.py

@@ -4,7 +4,7 @@
 import numpy as np
 
 from MAPLEAF.GNC import \
-    ScheduledGainPIDRocketMomentController
+    TableScheduledGainPIDRocketMomentController, EquationScheduledGainPIDRocketMomentController
 from MAPLEAF.GNC import Stabilizer
 from MAPLEAF.Motion import AngularVelocity, Quaternion, RigidBodyState, Vector
 from MAPLEAF.IO import SimDefinition
@@ -13,7 +13,7 @@
 
 class TestScheduledGainPIDRocketMomentController(unittest.TestCase):
     def setUp(self):
-        self.momentController = ScheduledGainPIDRocketMomentController("MAPLEAF/Examples/TabulatedData/testPIDControlLaw.txt", ["Mach", "Altitude"])
+        self.momentController = TableScheduledGainPIDRocketMomentController("MAPLEAF/Examples/TabulatedData/testPIDControlLaw.txt", ["Mach", "Altitude"])
         self.stabilizer = Stabilizer(Vector(0,0,1))
 
     def test_getOrientationErrorAndGetTargetOrientation(self):
@@ -128,6 +128,63 @@ def fakeAltitude(*args):
             # print(ExpectedM[i])
             self.assertAlmostEqual(calculatedMoments[i], ExpectedM[i])
 
+class TestEquationScheduledGainPIDROcketMomentController(unittest.TestCase):
+
+    def setUp(self):
+      parameterList = ["Mach", "Altitude"]
+      equationOrder = 2
+      self.momentController = EquationScheduledGainPIDRocketMomentController("MAPLEAF/Examples/TabulatedData/lateralPIDEquationCoeffs.txt",\
+        "MAPLEAF/Examples/TabulatedData/lateralPIDEquationCoeffs.txt", parameterList, equationOrder)
+
+      self.stabilizer = Stabilizer(Vector(0,0,1))
+
+    def test_getEquationCoefficientsFromTextFile(self):
+      result = self.momentController.pitchController.PcoefficientList
+      expectedResult = [1,2,3,4,5,6]
+
+      for i in range(len(expectedResult)):
+        self.assertAlmostEqual(result[i],expectedResult[i])
+
+    def test_getDesiredMoments(self):
+      # Basic spin case
+        pos = Vector(0,0,0)
+        vel = Vector(0,0,0)
+        orientation = Quaternion(axisOfRotation=Vector(0,0,1), angle=0.12)
+        targetOrientation = Quaternion(axisOfRotation=Vector(0,0,1), angle=0)
+        angularVelocity = AngularVelocity(axisOfRotation=Vector(0,0,1), angularVel=0)
+        rigidBodyState = RigidBodyState(pos, vel, orientation, angularVelocity)
+        expectedAngleError = np.array([ 0, 0, -0.12 ])
+
+        dt = 1
+        ExpectedPIDCoeffs = [[ 687, 687, 687], [ 687, 687, 687], [ 687, 687, 687]]
+
+        ExpectedDer = expectedAngleError / dt
+        ExpectedIntegral = expectedAngleError * dt / 2
+
+        ExpectedM = []
+        for i in range(3):
+            moment = ExpectedPIDCoeffs[i][0]*expectedAngleError[i] + ExpectedPIDCoeffs[i][1]*ExpectedIntegral[i] + ExpectedPIDCoeffs[i][2]*ExpectedDer[i]
+            ExpectedM.append(moment)
+
+        # Replace keyFunctions with these ones that return fixed values
+        def fakeMach(*args):
+            # MachNum = 1
+            return 1
+
+        def fakeAltitude(*args):
+            # Altitude = 10
+            return 10
+
+        self.momentController.parameterFetchFunctionList = [ fakeMach, fakeAltitude ]
+
+        calculatedMoments = self.momentController.getDesiredMoments(rigidBodyState, "fakeEnvironemt", targetOrientation, 0, dt)
+        for i in range(3):
+            # print(calculatedMoments[i])
+            # print(ExpectedM[i])
+            self.assertAlmostEqual(calculatedMoments[i], ExpectedM[i])
+
+
+
 class TestIdealMomentController(unittest.TestCase):
     def test_instantTurn(self):
         simulationDefinition = SimDefinition("MAPLEAF/Examples/Simulations/Canards.mapleaf", silent=True)

test/test_GNC/test_PID.py

@@ -1,8 +1,9 @@
 import unittest
 
 import numpy as np
+import sys
 
-from MAPLEAF.GNC import ScheduledGainPIDController, PIDController
+from MAPLEAF.GNC import PIDController, TableScheduledGainPIDController, EquationScheduledGainPIDController
 
 
 class TestPIDController(unittest.TestCase):
@@ -75,17 +76,69 @@ def test_updateMaxIntegral_vector(self):
         self.assertTrue(np.array_equal(newSetPoint, np.array([84, 205, 366])))
 
 
-class TestGainSchedulePIDController(unittest.TestCase):
+class TestTableScheduledGainPIDController(unittest.TestCase):
     def setUp(self):
-        self.ScheduledGainPID = ScheduledGainPIDController("MAPLEAF/Examples/TabulatedData/testPIDControlLaw.txt", 2, 2, 7)
+        self.TableScheduledGainPID = TableScheduledGainPIDController("MAPLEAF/Examples/TabulatedData/testPIDControlLaw.txt", 2, 2, 7)
 
     def test_getPIDCoeffs(self):
         MachNum, Alt = 0.15, 0
         expectedResult = np.array([ 7.5, 8.5, 9.5, 10.5, 11.5, 12.5 ])
-        result = self.ScheduledGainPID._getPIDCoeffs(MachNum, Alt)
+        result = self.TableScheduledGainPID._getPIDCoeffs(MachNum, Alt)
         self.assertTrue(np.allclose(result, expectedResult))
 
         MachNum, Alt = 0.1, 0.5
         expectedResult = np.array([ 4, 5, 6, 7.5, 8.5, 9.5 ])
-        result = self.ScheduledGainPID._getPIDCoeffs(MachNum, Alt)
-        self.assertTrue(np.allclose(result, expectedResult))
+        result = self.TableScheduledGainPID._getPIDCoeffs(MachNum, Alt)
+        self.assertTrue(np.allclose(result, expectedResult))
+
+class TestEquationScheduledGainPIDController(unittest.TestCase):
+  def setUp(self):
+    coefficientList = [[1, 2, 3, 4, 5, 6],
+                      [1, 2, 3, 4, 5, 6],
+                      [1, 2, 3, 4, 5, 6]]
+
+    parameterList = ["Mach Number", "Altitude"]
+
+    equationOrder = 2
+
+    self.equationScheduledGainPID = EquationScheduledGainPIDController(coefficientList, parameterList, equationOrder)
+
+  def test_updateCoefficientsFromEquation(self):
+
+    MachNum = 1
+    Altitude = 10
+
+    parameterValues = [MachNum, Altitude]
+    ExpectedResult = 1 + 2*MachNum + 3*Altitude + 4*(MachNum**2) + 5*MachNum*Altitude + 6*(Altitude**2)
+
+    self.equationScheduledGainPID.updateCoefficientsFromEquation(parameterValues)
+
+    P = self.equationScheduledGainPID.P
+    I = self.equationScheduledGainPID.I
+    D = self.equationScheduledGainPID.D
+
+    self.assertTrue(np.isclose(ExpectedResult,P))
+    self.assertTrue(np.isclose(ExpectedResult,I))
+    self.assertTrue(np.isclose(ExpectedResult,D))
+
+  def test_EquationScheduledPIDOutput(self):
+
+    Error = 1
+    dt = 1
+
+    MachNum = 1
+    Altitude = 10
+
+    parameterValues = [MachNum, Altitude]
+    Gains = 1 + 2*MachNum + 3*Altitude + 4*(MachNum**2) + 5*MachNum*Altitude + 6*(Altitude**2)
+
+    Derivative = (Error - 0)/dt
+    Integral = Error*dt/2
+
+    ExpectedResult = Gains*Error + Gains*Integral + Gains*Derivative
+
+    self.equationScheduledGainPID.updateCoefficientsFromEquation(parameterValues)
+
+    Output = self.equationScheduledGainPID.getNewSetPoint(Error,dt)
+
+    self.assertTrue(np.isclose(ExpectedResult,Output))