Module MAPLEAF.GNC

Guidance, Navigation, and Control functionality for 3D navigation (air vehicles). Main module is MAPLEAF.GNC.ControlSystems

'''
Guidance, Navigation, and Control functionality for 3D navigation (air vehicles).
Main module is `MAPLEAF.GNC.ControlSystems`

.. image:: https://upload.wikimedia.org/wikipedia/commons/thumb/e/ee/Homing_pigeon.jpg/1272px-Homing_pigeon.jpg
'''
# Make the classes in all submodules importable directly from MAPLEAF.Rocket

from .Actuators import *
from .PID import *
from .Navigation import *
from .actuatedSystem import *
from .MomentControllers import *
from .ControlSystems import *

subModules = [ actuatedSystem, Actuators, ControlSystems, MomentControllers, Navigation, PID ]

__all__ = []

for subModule in subModules:
    __all__ += subModule.__all__

Sub-modules

MAPLEAF.GNC.Actuators

Modelling of actuators, which can control things like the position of rocket fin angles…

MAPLEAF.GNC.ControlSystems

Classes that implement air vehicle control systems by tying together aNavigator, a …

MAPLEAF.GNC.MomentControllers

Moment controllers determine what overall moments should be applied to the rocket to remain on the course provided by the …

MAPLEAF.GNC.Navigation

Navigators determine where to go

MAPLEAF.GNC.PID

PID controllers control parts of the control system and adaptive simulation timestepping

MAPLEAF.GNC.actuatedSystem

The actuated system class initializes actuators, which are controlled by a ControlSystem Rocket component classes need …

Classes

class ActuatedSystem (nActuators)

Mixin-type class
Inherit from this class to add its functionality to a class

• ActuatedSystem.__init__ initializes actuators defined in the component's 'Actuator' subdictionary
• These actuators are then stored in self.actuatorList
• Child classes must call ActuatedSystem.__init__() to actually instantiate the actuators
• Expects that child classes also inherit from SubDictReader, and have already initialized the SubDictReader functionality
• Expects that child classes have a self.rocket attribute

Expand source code

class ActuatedSystem(abc.ABC):
    '''
            **Mixin-type class**  
            Inherit from this class to add its functionality to a class  
                
            * ActuatedSystem.\_\_init\_\_ initializes actuators defined in the component's 'Actuator' subdictionary  
            * These actuators are then stored in self.actuatorList  
            * Child classes must call ActuatedSystem.\_\_init\_\_() to actually instantiate the actuators
            * Expects that child classes also inherit from SubDictReader, and have already initialized the SubDictReader functionality
            * Expects that child classes have a self.rocket attribute
    '''
    def __init__(self, nActuators):

        # Initialize Actuator Models
        actuatorList = []
        for i in range(nActuators):
            actuatorResponseModel = self.componentDictReader.getString("Actuators.responseModel")
            if actuatorResponseModel == "FirstOrder":
                responseTime = self.componentDictReader.getFloat("Actuators.responseTime")
                maxDeflection = self.componentDictReader.tryGetFloat("Actuators.maxDeflection", defaultValue=None)
                minDeflection = self.componentDictReader.tryGetFloat("Actuators.minDeflection", defaultValue=None)
                actuator = FirstOrderActuator(responseTime, maxDeflection=maxDeflection, minDeflection=minDeflection)
                actuatorList.append(actuator)
            else:
                raise ValueError("Actuator response model {} not implemented. Try 'FirstOrder'".format(actuatorResponseModel))

        self.actuatorList = actuatorList

        # Initialize Actuator controller
        controllerType = self.componentDictReader.getString("Actuators.controller")
        if controllerType == "TableInterpolating":
            deflectionTablePath = self.componentDictReader.getString("Actuators.deflectionTablePath")

            # Get list of 'key' columns
            deflectionKeyColumns = self.componentDictReader.getString("Actuators.deflectionKeyColumns").split()
            noDesiredKeys = []
            for key in deflectionKeyColumns:
                if "Desired" not in key:
                    noDesiredKeys.append(key)
                else:
                    break

            # Check that no non-'desired' key columns after desired ones
            for i in range(len(noDesiredKeys), len(deflectionKeyColumns)):
                if "Desired" not in deflectionKeyColumns[i]:
                    raise ValueError("'Desired' columns such as DesiredMx must come last in deflectionKeyColumns")

            deflectionKeyFunctionVector = [ AeroParameters.stringToAeroFunctionMap[key] for key in noDesiredKeys ]

            self.actuatorController = TableInterpolatingActuatorController(deflectionTablePath, len(deflectionKeyColumns), deflectionKeyFunctionVector, actuatorList)
        else:
            raise ValueError("Actuator controller: {} not implemented. Try TableInterpolating.")

    @abc.abstractmethod
    def initializeActuators(self, controlSystem):
        ''' Concrete implementations of this function should call ActuatedSystem.\_\_init\_\_(self, nActuators) - implementations of this function only need to determine and pass in nActuators  '''        
        pass

Ancestors

• abc.ABC

Subclasses

• FinSet

Methods

def initializeActuators(self, controlSystem)

Concrete implementations of this function should call ActuatedSystem.__init__(self, nActuators) - implementations of this function only need to determine and pass in nActuators

Expand source code

@abc.abstractmethod
def initializeActuators(self, controlSystem):
    ''' Concrete implementations of this function should call ActuatedSystem.\_\_init\_\_(self, nActuators) - implementations of this function only need to determine and pass in nActuators  '''        
    pass

class Actuator

Interface for actuators. Actuators model the movement of a physical actuator (ex. servo, hydraulics). Target deflections are usually provided by the rocket's ControlSystem, usually obtained from a ActuatorController

Expand source code

class Actuator(abc.ABC):
    ''' 
        Interface for actuators.
        Actuators model the movement of a physical actuator (ex. servo, hydraulics).
        Target deflections are usually provided by the rocket's `MAPLEAF.GNC.ControlSystems.ControlSystem`, usually obtained from a `ActuatorController`
    '''
    @abc.abstractmethod
    def setTargetDeflection(self, newTarget, time):
        ''' Should return nothing, update setPoint for the actuator '''
        pass

    @abc.abstractmethod
    def getDeflection(self, time):
        ''' Should return the current deflection '''
        pass

    @abc.abstractmethod
    def getDeflectionDerivative(self, state, environment, time):
        ''' Should return the current deflection derivative '''
        pass

Ancestors

• abc.ABC

Subclasses

• FirstOrderActuator

Methods

def getDeflection(self, time)

Should return the current deflection

Expand source code

@abc.abstractmethod
def getDeflection(self, time):
    ''' Should return the current deflection '''
    pass

def getDeflectionDerivative(self, state, environment, time)

Should return the current deflection derivative

Expand source code

@abc.abstractmethod
def getDeflectionDerivative(self, state, environment, time):
    ''' Should return the current deflection derivative '''
    pass

def setTargetDeflection(self, newTarget, time)

Should return nothing, update setPoint for the actuator

Expand source code

@abc.abstractmethod
def setTargetDeflection(self, newTarget, time):
    ''' Should return nothing, update setPoint for the actuator '''
    pass

class ActuatorController

Interface for actuator controllers.
Actuator controllers are in charge of determining what actuator deflections are required to produce moments desired by the rocket's ControlSystem

Expand source code

class ActuatorController(abc.ABC):
    ''' 
        Interface for actuator controllers.   
        Actuator controllers are in charge of determining what actuator deflections are required to produce moments desired by the rocket's `MAPLEAF.GNC.ControlSystems.ControlSystem`
    '''
    @abc.abstractmethod
    def setTargetActuatorDeflections(self, desiredMoments, state, environment, time):
        ''' Should return nothing, control the appropriate rocket system to generate the desired moments '''
        pass

Ancestors

• abc.ABC

Subclasses

• TableInterpolatingActuatorController

Methods

def setTargetActuatorDeflections(self, desiredMoments, state, environment, time)

Should return nothing, control the appropriate rocket system to generate the desired moments

Expand source code

@abc.abstractmethod
def setTargetActuatorDeflections(self, desiredMoments, state, environment, time):
    ''' Should return nothing, control the appropriate rocket system to generate the desired moments '''
    pass

class ConstantGainPIDController (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

Expand source code

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)

Ancestors

• PIDController

Subclasses

• ConstantGainPIDRocketMomentController

class ConstantGainPIDRocketMomentController (Pxy, Ixy, Dxy, Pz, Iz, Dz)

Helper class that provides a standard way to create an ABC using inheritance.

Constant PID coefficient controller

Expand source code

class ConstantGainPIDRocketMomentController(MomentController, ConstantGainPIDController):
    def __init__(self, Pxy, Ixy, Dxy, Pz, Iz, Dz):
        '''
            Constant PID coefficient controller
        '''
        ConstantGainPIDController.__init__(self)
        
        P = np.array([Pxy, Pxy, Pz])
        I = np.array([Ixy, Ixy, Iz])
        D = np.array([Dxy, Dxy, Dz])
        self.updateCoefficients(P,I,D)

    def _getOrientationError(self, rocketState, targetOrientation):
        return np.array((targetOrientation / rocketState.orientation).toRotationVector())

    def getDesiredMoments(self, rocketState, environment, targetOrientation, time, dt):
        '''
            Inputs: 
                gainKeyList:        iterable of length nKeyColumns, containing the data required to interpolate the PIDxy and PIDz coefficients in the gain table
                rocketState:        must have .orientation attribute (Quaternion)
                targetOrientation:  (Quaternion)
                _:                  currently unused (time argument)
                dt:                 (numeric) time since last execution of the control system
        '''
        orientationError = self._getOrientationError(rocketState, targetOrientation)
        return self.getNewSetPoint(orientationError, dt)

Ancestors

• MomentController
• abc.ABC
• ConstantGainPIDController
• PIDController

Methods

def getDesiredMoments(self, rocketState, environment, targetOrientation, time, dt)

Inputs: gainKeyList: iterable of length nKeyColumns, containing the data required to interpolate the PIDxy and PIDz coefficients in the gain table rocketState: must have .orientation attribute (Quaternion) targetOrientation: (Quaternion) _: currently unused (time argument) dt: (numeric) time since last execution of the control system

Expand source code

def getDesiredMoments(self, rocketState, environment, targetOrientation, time, dt):
    '''
        Inputs: 
            gainKeyList:        iterable of length nKeyColumns, containing the data required to interpolate the PIDxy and PIDz coefficients in the gain table
            rocketState:        must have .orientation attribute (Quaternion)
            targetOrientation:  (Quaternion)
            _:                  currently unused (time argument)
            dt:                 (numeric) time since last execution of the control system
    '''
    orientationError = self._getOrientationError(rocketState, targetOrientation)
    return self.getNewSetPoint(orientationError, dt)

class ControlSystem

Interface for control systems

Expand source code

class ControlSystem(abc.ABC):
    ''' Interface for control systems '''

    @abc.abstractmethod
    def runControlLoopIfRequired(self, currentTime, rocketState, environment):
        ''' 
            Should check if the control loop needs to be run and run it if so. 
            Should return a list of actuator deflections (for logging)

            Reference to controlled system expected to be self.controlledSystem
            Controlled system expected to inherit from ActuatedSystem

            If the control system ran:
                Should return an iterable of new actuator position targets (in order consistent with log headers)
            If the control system didn't run:
                Should return False
        '''

    def calculateAngularError(self, currentOrientation, targetOrientation):

        errors = np.array((targetOrientation / currentOrientation).toRotationVector())
        pitchError = errors[0]
        yawError = errors[1]
        rollError = errors[2]

        return pitchError, yawError, rollError

Ancestors

• abc.ABC

Subclasses

• RocketControlSystem

Methods

def calculateAngularError(self, currentOrientation, targetOrientation)

Expand source code

def calculateAngularError(self, currentOrientation, targetOrientation):

    errors = np.array((targetOrientation / currentOrientation).toRotationVector())
    pitchError = errors[0]
    yawError = errors[1]
    rollError = errors[2]

    return pitchError, yawError, rollError

def runControlLoopIfRequired(self, currentTime, rocketState, environment)

Should check if the control loop needs to be run and run it if so. Should return a list of actuator deflections (for logging)

Reference to controlled system expected to be self.controlledSystem Controlled system expected to inherit from ActuatedSystem

If the control system ran: Should return an iterable of new actuator position targets (in order consistent with log headers) If the control system didn't run: Should return False

Expand source code

@abc.abstractmethod
def runControlLoopIfRequired(self, currentTime, rocketState, environment):
    ''' 
        Should check if the control loop needs to be run and run it if so. 
        Should return a list of actuator deflections (for logging)

        Reference to controlled system expected to be self.controlledSystem
        Controlled system expected to inherit from ActuatedSystem

        If the control system ran:
            Should return an iterable of new actuator position targets (in order consistent with log headers)
        If the control system didn't run:
            Should return False
    '''

class FirstOrderActuator (responseTime, initialDeflection=0, initialTime=0, maxDeflection=None, minDeflection=None)

First-order system - simple enought that its motion between control loops can be determined analytically. Motion does not need to be integrated

Basically just a version of the FirstOrderSystem class that internally stores state information

Expand source code

class FirstOrderActuator(Actuator, FirstOrderSystem):
    '''
        First-order system - simple enought that its motion between control loops can be determined analytically.
        Motion does not need to be integrated

        Basically just a version of the FirstOrderSystem class that internally stores state information
    '''
    def __init__(self, responseTime, initialDeflection=0, initialTime=0, maxDeflection=None, minDeflection=None):
        self.responseTime = responseTime
        self.lastDeflection = initialDeflection
        self.lastTime = 0
        self.targetDeflection = 0
        self.maxDeflection = maxDeflection
        self.minDeflection = minDeflection

    def setTargetDeflection(self, newTarget, time):
        # Record current deflection and time
        self.lastDeflection = self.getDeflection(time)
        self.lastTime = time

        # Apply limiters to new target
        if self.maxDeflection != None:
            newTarget = min(newTarget, self.maxDeflection)
        if self.minDeflection != None:
            newTarget = max(newTarget, self.minDeflection)

        # Set new target
        self.targetDeflection = newTarget

    def getDeflection(self, time):
        return self.getPosition(time, self.lastTime, self.lastDeflection, self.targetDeflection)

    def getDeflectionDerivative(self, _, _2, _3):
        ''' Not required for first-order actuator - deflections can be integrated analytically '''
        pass

Ancestors

• Actuator
• abc.ABC
• FirstOrderSystem

Methods

def getDeflectionDerivative(self, _, _2, _3)

Not required for first-order actuator - deflections can be integrated analytically

Expand source code

def getDeflectionDerivative(self, _, _2, _3):
    ''' Not required for first-order actuator - deflections can be integrated analytically '''
    pass

Inherited members

• Actuator:
  • getDeflection
  • setTargetDeflection

class FirstOrderSystem (responseTime)

Expand source code

class FirstOrderSystem():
    def __init__(self, responseTime):
        self.responseTime = responseTime

    def getPosition(self, currentTime, lastTime, lastPosition, lastTarget):
        dt = currentTime - lastTime
        if dt < -1e-17: # Exception check here because this would pass through the exponent expression below without causing an error
            raise ValueError("Current time ({}) must be after lastTime ({})".format(currentTime, lastTime))
            
        if dt < 0:
            dt = 0

        lastError = lastTarget - lastPosition
        errorFractionRemoved = (1 - e**(-dt/self.responseTime))
        newPosition = lastPosition + lastError*errorFractionRemoved

        return newPosition

Subclasses

• FirstOrderActuator

Methods

def getPosition(self, currentTime, lastTime, lastPosition, lastTarget)

Expand source code

def getPosition(self, currentTime, lastTime, lastPosition, lastTarget):
    dt = currentTime - lastTime
    if dt < -1e-17: # Exception check here because this would pass through the exponent expression below without causing an error
        raise ValueError("Current time ({}) must be after lastTime ({})".format(currentTime, lastTime))
        
    if dt < 0:
        dt = 0

    lastError = lastTarget - lastPosition
    errorFractionRemoved = (1 - e**(-dt/self.responseTime))
    newPosition = lastPosition + lastError*errorFractionRemoved

    return newPosition

class IdealMomentController (rocket)

Expand source code

class IdealMomentController():
    def __init__(self, rocket):
        self.rocket = rocket

    def getDesiredMoments(self, rocketState, environment, targetOrientation, time, dt):
        """ Exactly match the desired target orientation and set the rockets angular velocity to zero """
        self.rocket.rigidBody.state.orientation = targetOrientation
        self.rocket.rigidBody.state.angularVelocity = AngularVelocity(rotationVector=Vector(0,0,0))

Methods

def getDesiredMoments(self, rocketState, environment, targetOrientation, time, dt)

Exactly match the desired target orientation and set the rockets angular velocity to zero

Expand source code

def getDesiredMoments(self, rocketState, environment, targetOrientation, time, dt):
    """ Exactly match the desired target orientation and set the rockets angular velocity to zero """
    self.rocket.rigidBody.state.orientation = targetOrientation
    self.rocket.rigidBody.state.angularVelocity = AngularVelocity(rotationVector=Vector(0,0,0))

class MomentController

Helper class that provides a standard way to create an ABC using inheritance.

Expand source code

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

Ancestors

• abc.ABC

Subclasses

• ConstantGainPIDRocketMomentController
• ScheduledGainPIDRocketMomentController

Methods

def getDesiredMoments(self, rocketState, environment, targetOrientation, time, dt)

Should return a list [ desired x-axis, y-axis, and z-axis ] moments

Expand source code

@abc.abstractmethod
def getDesiredMoments(self, rocketState, environment, targetOrientation, time, dt):
    ''' Should return a list [ desired x-axis, y-axis, and z-axis ] moments '''

class Navigator

Helper class that provides a standard way to create an ABC using inheritance.

Expand source code

class Navigator(abc.ABC):
    @abc.abstractmethod
    def getTargetOrientation(self, rocketState, targetState, time):
        ''' Should return a target orientation quaternion '''
        pass

Ancestors

• abc.ABC

Subclasses

• Stabilizer

Methods

def getTargetOrientation(self, rocketState, targetState, time)

Should return a target orientation quaternion

Expand source code

@abc.abstractmethod
def getTargetOrientation(self, rocketState, targetState, time):
    ''' Should return a target orientation quaternion '''
    pass

class PIDController (P, I, D, initialError=0, maxIntegral=None)

To make this PID controller work for vectors of values, pass in np arrays - they are added elementwise by default

Expand source code

class PIDController():

    def __init__(self, P, I, D, initialError=0, maxIntegral=None):
        ''' To make this PID controller work for vectors of values, pass in np arrays 
                - they are added elementwise by default
        '''
        self.P = P
        self.I = I
        self.D = D
        self.lastError = initialError
        self.errorIntegral = initialError * 0 # Initialized like this in case value passed in is an array
        self.updateMaxIntegral(maxIntegral)

    def updateMaxIntegral(self, maxIntegral):
        if maxIntegral is not None:
            self.maxIntegralMagnitude = abs(maxIntegral)
        else:
            self.maxIntegralMagnitude = maxIntegral

    def getNewSetPoint(self, currentError, dt):
        # Calculate derivative
        derivative = (currentError - self.lastError) / dt
        
        # Calculate integral term
        self.errorIntegral = self.errorIntegral + (currentError + self.lastError)*dt / 2

        # Cap the magnitude of the integral term if necessary
        if self.maxIntegralMagnitude is not None:
            try:
                # Check if vector valued
                testIter = iter(self.errorIntegral)
                for i in range(len(self.errorIntegral)):
                    if abs(self.errorIntegral[i]) > self.maxIntegralMagnitude[i]:
                        self.errorIntegral[i] = self.errorIntegral[i] * self.maxIntegralMagnitude[i] / abs(self.errorIntegral[i])

            except TypeError:
                # Scalar valued
                if abs(self.errorIntegral) > self.maxIntegralMagnitude:
                    self.errorIntegral = self.errorIntegral * self.maxIntegralMagnitude / abs(self.errorIntegral)

        # Store error for next calculation
        self.lastError = currentError

        return self.P*currentError + self.I*self.errorIntegral + self.D*derivative

    def updateCoefficients(self, P, I, D, maxIntegral=None):
        self.P = P
        self.I = I
        self.D = D
        self.updateMaxIntegral(maxIntegral)

    def resetIntegral(self):
        self.errorIntegral = self.lastError * 0 # Done to handle arbitrary size np arrays

Subclasses

• ConstantGainPIDController
• ScheduledGainPIDController

Methods

def getNewSetPoint(self, currentError, dt)

Expand source code

def getNewSetPoint(self, currentError, dt):
    # Calculate derivative
    derivative = (currentError - self.lastError) / dt
    
    # Calculate integral term
    self.errorIntegral = self.errorIntegral + (currentError + self.lastError)*dt / 2

    # Cap the magnitude of the integral term if necessary
    if self.maxIntegralMagnitude is not None:
        try:
            # Check if vector valued
            testIter = iter(self.errorIntegral)
            for i in range(len(self.errorIntegral)):
                if abs(self.errorIntegral[i]) > self.maxIntegralMagnitude[i]:
                    self.errorIntegral[i] = self.errorIntegral[i] * self.maxIntegralMagnitude[i] / abs(self.errorIntegral[i])

        except TypeError:
            # Scalar valued
            if abs(self.errorIntegral) > self.maxIntegralMagnitude:
                self.errorIntegral = self.errorIntegral * self.maxIntegralMagnitude / abs(self.errorIntegral)

    # Store error for next calculation
    self.lastError = currentError

    return self.P*currentError + self.I*self.errorIntegral + self.D*derivative

def resetIntegral(self)

Expand source code

def resetIntegral(self):
    self.errorIntegral = self.lastError * 0 # Done to handle arbitrary size np arrays

def updateCoefficients(self, P, I, D, maxIntegral=None)

Expand source code

def updateCoefficients(self, P, I, D, maxIntegral=None):
    self.P = P
    self.I = I
    self.D = D
    self.updateMaxIntegral(maxIntegral)

def updateMaxIntegral(self, maxIntegral)

Expand source code

def updateMaxIntegral(self, maxIntegral):
    if maxIntegral is not None:
        self.maxIntegralMagnitude = abs(maxIntegral)
    else:
        self.maxIntegralMagnitude = maxIntegral

class RocketControlSystem (controlSystemDictReader, rocket, initTime=0, log=False, silent=False)

Simplest possible control system for a rocket

Expand source code

class RocketControlSystem(ControlSystem, SubDictReader):
    ''' Simplest possible control system for a rocket '''

    def __init__(self, controlSystemDictReader, rocket, initTime=0, log=False, silent=False):
        self.rocket = rocket
        self.controlSystemDictReader = controlSystemDictReader
        self.lastControlLoopRunTime = initTime
        self.timeSteppingModified = False # set to True if the sim's time stepping has been constrained by the control system update rate (in self._checkSimTimeStepping)
        self.silent = silent

        ### Create Navigator ###
        desiredFlightDirection = controlSystemDictReader.getVector("desiredFlightDirection")
        self.navigator = Stabilizer(desiredFlightDirection)

        ### Create Moment Controller ###
        momentControllerType = controlSystemDictReader.getString("MomentController.Type")
        if momentControllerType == "ConstantGainPIDRocket":
            Pxy = controlSystemDictReader.getFloat("MomentController.Pxy")
            Ixy = controlSystemDictReader.getFloat("MomentController.Ixy")
            Dxy = controlSystemDictReader.getFloat("MomentController.Dxy")
            Pz = controlSystemDictReader.getFloat("MomentController.Pz")
            Iz = controlSystemDictReader.getFloat("MomentController.Iz")
            Dz = controlSystemDictReader.getFloat("MomentController.Dz")
            self.momentController = ConstantGainPIDRocketMomentController(Pxy,Ixy,Dxy,Pz,Iz,Dz)
        elif momentControllerType == "ScheduledGainPIDRocket":
            gainTableFilePath = controlSystemDictReader.getString("MomentController.gainTableFilePath")
            keyColumnNames = controlSystemDictReader.getString("MomentController.scheduledBy").split()
            self.momentController = ScheduledGainPIDRocketMomentController(gainTableFilePath, keyColumnNames)
        elif momentControllerType == "IdealMomentController":
            self.momentController = IdealMomentController(self.rocket)
        else:
            raise ValueError("Moment Controller Type: {} not implemented. Try ScheduledGainPIDRocket or IdealMomentController".format(momentControllerType))

        ### Set update rate ###
        if momentControllerType == "IdealMomentController":
            # When using the ideal controller, update the rocket's orientation as often as possible
            self.updateRate = 0.0
        else:
            # Otherwise model a real control system
            self.updateRate = controlSystemDictReader.getFloat("updateRate")

        self.controlledSystem = None
        if momentControllerType != "IdealMomentController":
            ### Get reference to the controlled system ###
            controlledSystemPath = controlSystemDictReader.getString("controlledSystem")
            for stage in rocket.stages:
                for rocketComponent in stage.components:
                    if rocketComponent.componentDictReader.simDefDictPathToReadFrom == controlledSystemPath:
                        self.controlledSystem = rocketComponent
                        break

            if self.controlledSystem == None:
                simDefinitionFile = controlSystemDictReader.simDefinition.fileName
                raise ValueError("Rocket Component: {} not found in {}".format(controlledSystemPath, simDefinitionFile))

            self.controlledSystem.initializeActuators(self)

        self._checkSimTimeStepping()

        ### Set up logging if requested ###
        if log:
            self.log = Log()
            self.log.addColumns(["PitchError", "YawError", "RollError"])
            
            if self.controlledSystem != None:
                nActuators = len(self.controlledSystem.actuatorList)
                targetDeflectionColumnNames = [ "Actuator_{}_TargetDeflection".format(i) for i in range(nActuators) ]
                self.log.addColumns(targetDeflectionColumnNames)

                deflectionColumnNames = [ "Actuator_{}_Deflection".format(i) for i in range(nActuators) ]
                self.log.addColumns(deflectionColumnNames)

        else:
            self.log = None

    def _checkSimTimeStepping(self):
        # If the update rate is zero, control system is simply run once per time step
        if self.updateRate > 0:
            # Since a discrete update rate has been specified, we need to ensure that the simulation time step is an integer divisor of the control system time step
            
            # Disable adaptive time stepping during the ascent portion of the flight (if it's enabled)
            timeDiscretization = self.controlSystemDictReader.getString("SimControl.timeDiscretization")
            if "Adaptive" in timeDiscretization:
                if not self.silent:
                    print("WARNING: Time stepping conflict between adaptive-time-stepping Runge-Kutta method and fixed control system update rate")
                    print("Disabling adaptive time stepping for ascent portion of flight. Will re-enable if/when recovery system deploys.")
                    print("Switching to RK4 time stepping")
                self.rocket.rigidBody.integrate = integratorFactory(integrationMethod='RK4')
                self.timeSteppingModified = True

            # Adjust the sim time step to be an even divisor of the control system time step
            controlTimeStep = 1/self.updateRate
            self.controlTimeStep = controlTimeStep
            originalSimTimeStep = self.controlSystemDictReader.getFloat("SimControl.timeStep")
            self.originalSimTimeStep = originalSimTimeStep
            if controlTimeStep / originalSimTimeStep != round(controlTimeStep / originalSimTimeStep):
                recommendedSimTimeStep = controlTimeStep / max(1, round(controlTimeStep / originalSimTimeStep))
                if not self.silent:
                    print("WARNING: Selected time step ({}) does not divide the control system time step ({}) Into an integer number of time steps.".format(originalSimTimeStep, controlTimeStep))
                    print("Adjusting time step to: {}".format(recommendedSimTimeStep))
                    print("")
                    
                # This relies on the SimRunner reading the time step size from the sim definition after intializing the rocket
                    # To make this more robust, have the rocket perform a timestep size check every single time step
                self.controlSystemDictReader.simDefinition.setValue("SimControl.timeStep", str(recommendedSimTimeStep))
                self.timeSteppingModified = True
            
        else:
            self.controlTimeStep = 0

    def restoreOriginalTimeStepping(self):
        '''
            Should get called whenever the control system is disabled:
                1. If a stage containing the controlled system is dropped
                2. If the rocket recovery system is deployed
        '''
        if self.timeSteppingModified:
            # This currently won't do anything because the time step is saved in an array by the Simulation - might be useful in the future
            self.simDefinition.setValue("SimControl.timeStep", str(self.originalSimTimeStep))

            # This will re-initialize the integrator to match that originally selected in the sim definition file
            originalTimeIntegrationMethod = self.rocketDictReader.getValue("SimControl.timeDiscretization")
            print("Restoring original time discretization method ({})".format(originalTimeIntegrationMethod))
            self.rocket.rigidBody.integrate = integratorFactory(integrationMethod=originalTimeIntegrationMethod, simDefinition=self.controlDctReaders.simDefinition)

    def runControlLoopIfRequired(self, currentTime, rocketState, environment):
        # Run control loop if enough time has passed

        if (currentTime - self.lastControlLoopRunTime) + 0.0000000001 >= self.controlTimeStep:
            dt = currentTime - self.lastControlLoopRunTime

            # Figure out where to go/point
            targetOrientation = self.navigator.getTargetOrientation(rocketState, "notRequired", currentTime)

            # Figure out what moment needs to be applied
            desiredMoments = self.momentController.getDesiredMoments(rocketState, environment, targetOrientation, currentTime, dt)
            
            if self.controlledSystem != None:
                # Apply it by actuating the controlled system
                newActuatorPositionTargets = self.controlledSystem.actuatorController.setTargetActuatorDeflections(desiredMoments, rocketState, environment, currentTime)
            else:
                # End up here if using the ideal moment controller, no actuators to control
                    # Desired moments will be applied instantly without modeling the dynamics of the system that applies them
                newActuatorPositionTargets = [0]

            self.lastControlLoopRunTime = currentTime

            pitchError, yawError, rollError = self.calculateAngularError(rocketState.orientation,targetOrientation)

            if self.log is not None:
                self.log.newLogRow(currentTime)
                self.log.logValue("PitchError", pitchError)
                self.log.logValue("YawError", yawError)
                self.log.logValue("RollError", rollError)

                if self.controlledSystem != None:
                    for i in range(len(newActuatorPositionTargets)):
                        targetDeflectionColumnName = "Actuator_{}_TargetDeflection".format(i)
                        self.log.logValue(targetDeflectionColumnName, newActuatorPositionTargets[i])

                        deflectionColumnName = "Actuator_{}_Deflection".format(i)
                        currentDeflection = self.controlledSystem.actuatorController.actuatorList[i].getDeflection(currentTime)
                        self.log.logValue(deflectionColumnName, currentDeflection)

            return newActuatorPositionTargets
        else:
            return False

    def writeLogsToFile(self, directory="."):
        controlSystemPath = self.controlSystemDictReader.simDefDictPathToReadFrom.replace(".", "_")
        path = os.path.join(directory, "{}_Log.csv".format(controlSystemPath))
        self.log.writeToCSV(path)

        return path

Ancestors

• ControlSystem
• abc.ABC
• SubDictReader

Methods

def restoreOriginalTimeStepping(self)

Should get called whenever the control system is disabled: 1. If a stage containing the controlled system is dropped 2. If the rocket recovery system is deployed

Expand source code

def restoreOriginalTimeStepping(self):
    '''
        Should get called whenever the control system is disabled:
            1. If a stage containing the controlled system is dropped
            2. If the rocket recovery system is deployed
    '''
    if self.timeSteppingModified:
        # This currently won't do anything because the time step is saved in an array by the Simulation - might be useful in the future
        self.simDefinition.setValue("SimControl.timeStep", str(self.originalSimTimeStep))

        # This will re-initialize the integrator to match that originally selected in the sim definition file
        originalTimeIntegrationMethod = self.rocketDictReader.getValue("SimControl.timeDiscretization")
        print("Restoring original time discretization method ({})".format(originalTimeIntegrationMethod))
        self.rocket.rigidBody.integrate = integratorFactory(integrationMethod=originalTimeIntegrationMethod, simDefinition=self.controlDctReaders.simDefinition)

def writeLogsToFile(self, directory='.')

Expand source code

def writeLogsToFile(self, directory="."):
    controlSystemPath = self.controlSystemDictReader.simDefDictPathToReadFrom.replace(".", "_")
    path = os.path.join(directory, "{}_Log.csv".format(controlSystemPath))
    self.log.writeToCSV(path)

    return path

Inherited members

• ControlSystem:
  • runControlLoopIfRequired
• SubDictReader:
  • getString

class ScheduledGainPIDController (gainTableFilePath, nKeyColumns=2, PCol=3, DCol=5, initialError=0, maxIntegral=None)

Inputs

gainTableFilePath: (string) Path to gain table text file ex: './MAPLEAF/Examples/TabulatedData/constPIDCoeffs.txt' nKeyColumns: (int) Number of 'key' columns (independent variables). Key columns are assumed to be the nKeyColumns leftmost ones PCol: (int) zero-indexed column number of P Coefficient 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

Expand source code

class ScheduledGainPIDController(PIDController):
    def __init__(self, gainTableFilePath, nKeyColumns=2, PCol=3, DCol=5, initialError=0, maxIntegral=None):
        '''
            Inputs:
                gainTableFilePath:  (string) Path to gain table text file ex: './MAPLEAF/Examples/TabulatedData/constPIDCoeffs.txt'
                nKeyColumns:        (int) Number of 'key' columns (independent variables). Key columns are assumed to be the nKeyColumns leftmost ones
                PCol:               (int) zero-indexed column number of P Coefficient
                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, 0,0,0, initialError=initialError, maxIntegral=maxIntegral)

        # Columns are: Mach, Altitude(ASL), P1, I1, D1
        pidData = np.loadtxt(gainTableFilePath, skiprows=1)
        keys = pidData[:,0:nKeyColumns]
        pidData = pidData[:,PCol:DCol+1]

        #Create interpolation function for PID coefficients
        self._getPIDCoeffs = NoNaNLinearNDInterpolator(keys, pidData)

    def updateCoefficientsFromGainTable(self, keyList):
        P, I, D = self._getPIDCoeffs(keyList)
        self.updateCoefficients(P, I, D)

Ancestors

• PIDController

Subclasses

• ScheduledGainPIDRocketMomentController

Methods

def updateCoefficientsFromGainTable(self, keyList)

Expand source code

def updateCoefficientsFromGainTable(self, keyList):
    P, I, D = self._getPIDCoeffs(keyList)
    self.updateCoefficients(P, I, D)

class ScheduledGainPIDRocketMomentController (gainTableFilePath, keyColumnNames)

Helper class that provides a standard way to create an ABC using inheritance.

Assumes the longitudinal (Pitch/Yaw) PID coefficients are in columns nKeyColumns:nKeyColumns+2 Assumes the roll PID coefficients are in columns nKeyColumns+3:nKeyColumns+5 Sample gain file: MAPLEAF/Examples/TabulatedData/testPIDCoeffs.txt

Expand source code

class ScheduledGainPIDRocketMomentController(MomentController, ScheduledGainPIDController):
    def __init__(self, gainTableFilePath, keyColumnNames):
        '''
            Assumes the longitudinal (Pitch/Yaw) PID coefficients are in columns nKeyColumns:nKeyColumns+2
            Assumes the roll PID coefficients are in columns nKeyColumns+3:nKeyColumns+5
            Sample gain file: MAPLEAF/Examples/TabulatedData/testPIDCoeffs.txt
        '''
        self.keyFunctionList = [ AeroParameters.stringToAeroFunctionMap[x] for x in keyColumnNames ]
        nKeyColumns = len(keyColumnNames)
        ScheduledGainPIDController.__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 '''
        Pxy, Ixy, Dxy, Pz, Iz, Dz = self._getPIDCoeffs(*keyList)

        # Coefficient for each of P, I, and D are: Longitudinal, Longitudinal, Roll
        # Operates the internal PID controller in vector-mode with numpy arrays (using their elementwise operations)
        P = np.array([Pxy, Pxy, Pz])
        I = np.array([Ixy, Ixy, Iz])
        D = np.array([Dxy, Dxy, Dz])
        self.updateCoefficients(P, I, D)

    def _getOrientationError(self, rocketState, targetOrientation):
        return np.array((targetOrientation / rocketState.orientation).toRotationVector())

    def getDesiredMoments(self, rocketState, environment, targetOrientation, time, dt):
        '''
            Inputs: 
                gainKeyList:        iterable of length nKeyColumns, containing the data required to interpolate the PIDxy and PIDz coefficients in the gain table
                rocketState:        must have .orientation attribute (Quaternion)
                targetOrientation:  (Quaternion)
                _:                  currently unused (time argument)
                dt:                 (numeric) time since last execution of the control system
        '''
        orientationError = self._getOrientationError(rocketState, targetOrientation)
        gainKeyList = AeroParameters.getAeroPropertiesList(self.keyFunctionList, rocketState, environment)
        self.updateCoefficientsFromGainTable(gainKeyList)
        return self.getNewSetPoint(orientationError, dt)

Ancestors

• MomentController
• abc.ABC
• ScheduledGainPIDController
• PIDController

Methods

def getDesiredMoments(self, rocketState, environment, targetOrientation, time, dt)

Inputs: gainKeyList: iterable of length nKeyColumns, containing the data required to interpolate the PIDxy and PIDz coefficients in the gain table rocketState: must have .orientation attribute (Quaternion) targetOrientation: (Quaternion) _: currently unused (time argument) dt: (numeric) time since last execution of the control system

Expand source code

def getDesiredMoments(self, rocketState, environment, targetOrientation, time, dt):
    '''
        Inputs: 
            gainKeyList:        iterable of length nKeyColumns, containing the data required to interpolate the PIDxy and PIDz coefficients in the gain table
            rocketState:        must have .orientation attribute (Quaternion)
            targetOrientation:  (Quaternion)
            _:                  currently unused (time argument)
            dt:                 (numeric) time since last execution of the control system
    '''
    orientationError = self._getOrientationError(rocketState, targetOrientation)
    gainKeyList = AeroParameters.getAeroPropertiesList(self.keyFunctionList, rocketState, environment)
    self.updateCoefficientsFromGainTable(gainKeyList)
    return self.getNewSetPoint(orientationError, dt)

def updateCoefficientsFromGainTable(self, keyList)

Overriding parent class method to enable separate longitudinal and roll coefficients in a single controller

Expand source code

def updateCoefficientsFromGainTable(self, keyList):
    ''' Overriding parent class method to enable separate longitudinal and roll coefficients in a single controller '''
    Pxy, Ixy, Dxy, Pz, Iz, Dz = self._getPIDCoeffs(*keyList)

    # Coefficient for each of P, I, and D are: Longitudinal, Longitudinal, Roll
    # Operates the internal PID controller in vector-mode with numpy arrays (using their elementwise operations)
    P = np.array([Pxy, Pxy, Pz])
    I = np.array([Ixy, Ixy, Iz])
    D = np.array([Dxy, Dxy, Dz])
    self.updateCoefficients(P, I, D)

class Stabilizer (desiredFlightDirection)

Helper class that provides a standard way to create an ABC using inheritance.

Expand source code

class Stabilizer(Navigator):
    def __init__(self, desiredFlightDirection):
        # Calculate target orientation
        direction = desiredFlightDirection.normalize()
        angleFromVertical = Vector(0,0,1).angle(direction)
        rotationAxis = Vector(0,0,1).crossProduct(direction)
        self.targetOrientation = Quaternion(rotationAxis, angleFromVertical)

    def getTargetOrientation(self, rocketState, targetState, time):
        return self.targetOrientation

Ancestors

• Navigator
• abc.ABC

Inherited members

• Navigator:
  • getTargetOrientation

class TableInterpolatingActuatorController (deflectionTableFilePath, nKeyColumns, keyFunctionList, actuatorList)

Controls actuators

Expand source code

class TableInterpolatingActuatorController(ActuatorController):
    '''
        Controls actuators 
    '''
    def __init__(self, deflectionTableFilePath, nKeyColumns, keyFunctionList, actuatorList):
        self.actuatorList = actuatorList
        self.keyFunctionList = keyFunctionList
        self.deflectionTablePath = deflectionTableFilePath

        # Load actuator-deflection data table
        deflData = np.loadtxt(deflectionTableFilePath, skiprows=1)
        keys = deflData[:,0:nKeyColumns]
        deflData = deflData[:,nKeyColumns:]

        # Check that number of actuators matches number of deflection entries in the deflection tables
        nActuators = len(actuatorList)
        nDeflectionTableEntries = len(deflData[0,:])
        if nActuators != nDeflectionTableEntries:
            raise ValueError("Number of actuators: {}, must match number of actuator deflection columns in deflection table: {}".format(nActuators, nDeflectionTableEntries))

        # Create interpolation function for fin deflections
        self._getPositionTargets = NoNaNLinearNDInterpolator(keys, deflData)

    def setTargetActuatorDeflections(self, desiredMoments, state, environment, time):
        '''
            Inputs:
                desiredMoments: (3-length iterable) contains desired moments about the x,y,and z axes
                time:           time at which the moments are requested (time of current control loop execution)
        '''
        # Construct key vector, starting with non-moment components:
        keyVector = AeroParameters.getAeroPropertiesList(self.keyFunctionList, state, environment)
        for desiredMoment in desiredMoments:
            keyVector.append(desiredMoment)

        newActuatorPositionTargets = self._getPositionTargets(*keyVector)

        for i in range(len(self.actuatorList)):
            self.actuatorList[i].setTargetDeflection(newActuatorPositionTargets[i], time)

        return list(newActuatorPositionTargets)

Ancestors

• ActuatorController
• abc.ABC

Methods

def setTargetActuatorDeflections(self, desiredMoments, state, environment, time)

Inputs

desiredMoments: (3-length iterable) contains desired moments about the x,y,and z axes time: time at which the moments are requested (time of current control loop execution)

Expand source code

def setTargetActuatorDeflections(self, desiredMoments, state, environment, time):
    '''
        Inputs:
            desiredMoments: (3-length iterable) contains desired moments about the x,y,and z axes
            time:           time at which the moments are requested (time of current control loop execution)
    '''
    # Construct key vector, starting with non-moment components:
    keyVector = AeroParameters.getAeroPropertiesList(self.keyFunctionList, state, environment)
    for desiredMoment in desiredMoments:
        keyVector.append(desiredMoment)

    newActuatorPositionTargets = self._getPositionTargets(*keyVector)

    for i in range(len(self.actuatorList)):
        self.actuatorList[i].setTargetDeflection(newActuatorPositionTargets[i], time)

    return list(newActuatorPositionTargets)