diff --git a/MAPLEAF/Examples/Simulations/Canards.mapleaf b/MAPLEAF/Examples/Simulations/Canards.mapleaf index 8c430c6d..1a2095b0 100644 --- a/MAPLEAF/Examples/Simulations/Canards.mapleaf +++ b/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 } diff --git a/MAPLEAF/Examples/Simulations/EquationGainScheduled.mapleaf b/MAPLEAF/Examples/Simulations/EquationGainScheduled.mapleaf new file mode 100644 index 00000000..d668dd0b --- /dev/null +++ b/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 + } + } +} \ No newline at end of file diff --git a/MAPLEAF/Examples/TabulatedData/lateralPIDEquationCoeffs.txt b/MAPLEAF/Examples/TabulatedData/lateralPIDEquationCoeffs.txt new file mode 100644 index 00000000..553bbf27 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/MAPLEAF/Examples/TabulatedData/longitudinalPIDEquationCoeffs.txt b/MAPLEAF/Examples/TabulatedData/longitudinalPIDEquationCoeffs.txt new file mode 100644 index 00000000..553bbf27 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/MAPLEAF/GNC/ControlSystems.py b/MAPLEAF/GNC/ControlSystems.py index e75a93c3..02474061 100644 --- a/MAPLEAF/GNC/ControlSystems.py +++ b/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": diff --git a/MAPLEAF/GNC/MomentControllers.py b/MAPLEAF/GNC/MomentControllers.py index 84e00336..4bff88f2 100644 --- a/MAPLEAF/GNC/MomentControllers.py +++ b/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): ''' diff --git a/MAPLEAF/GNC/PID.py b/MAPLEAF/GNC/PID.py index 8a7a5967..dd777b61 100644 --- a/MAPLEAF/GNC/PID.py +++ b/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) \ No newline at end of file + 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) diff --git a/MAPLEAF/IO/subDictReader.py b/MAPLEAF/IO/subDictReader.py index de24b578..a816ae83 100644 --- a/MAPLEAF/IO/subDictReader.py +++ b/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:] + diff --git a/MAPLEAF/SimulationRunners/Batch.py b/MAPLEAF/SimulationRunners/Batch.py index 22239b37..dd90c172 100644 --- a/MAPLEAF/SimulationRunners/Batch.py +++ b/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 diff --git a/MAPLEAF/SimulationRunners/SingleSimulations.py b/MAPLEAF/SimulationRunners/SingleSimulations.py index 3f348a7a..89e9916b 100644 --- a/MAPLEAF/SimulationRunners/SingleSimulations.py +++ b/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) diff --git a/test/test_GNC/test_MomentControllers.py b/test/test_GNC/test_MomentControllers.py index 23fe902a..12facd52 100644 --- a/test/test_GNC/test_MomentControllers.py +++ b/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) diff --git a/test/test_GNC/test_PID.py b/test/test_GNC/test_PID.py index c666b020..74c6ea7d 100644 --- a/test/test_GNC/test_PID.py +++ b/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)) \ No newline at end of file + 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)) \ No newline at end of file