Module MAPLEAF.Motion

Generalized Rigid body motion integration functionality. Main class (6Dof) is RigidBody.
Fundamental data types used throughout the simulator defined in:

• Vector
• Quaternion - represents orientation
• AngularVelocity
• Inertia - stores component masses and moments of inertia
• ForceMomentSystem - stores a repositionable force-moment system

3Dof and 6DoF Rigid body states are composed of these fundamental data types and defined in MAPLEAF.Motion.RigidBodyStates

Generalized constant and adaptive time stepping integrators are defined in MAPLEAF.Motion.Integration

MAPLEAF.Motion does not rely on any of MAPLEAF's other packages.

'''
Generalized Rigid body motion integration functionality.
Main class (6Dof) is `MAPLEAF.Motion.RigidBody`.  
Fundamental data types used throughout the simulator defined in:

* `Vector`
* `Quaternion` - represents orientation
* `AngularVelocity` 
* `Inertia` - stores component masses and moments of inertia
* `ForceMomentSystem`  - stores a repositionable force-moment system

3Dof and 6DoF Rigid body states are composed of these fundamental data types and defined in `RigidBodyStates`

Generalized constant and adaptive time stepping integrators are defined in `Integration`

MAPLEAF.Motion does not rely on any of MAPLEAF's other packages.

.. image:: https://www.researchgate.net/profile/Alireza_Abbaspour2/publication/326452421/figure/fig1/AS:701040399753217@1544152464477/The-aircraft-coordinate-system-34.jpg
'''
# Make the classes in all submodules importable directly from MAPLEAF.Rocket
from .CythonVector import *
from .CythonQuaternion import *
from .CythonAngularVelocity import *
from .Integration import *
from .Interpolation import *
from .forceMomentSystem import ForceMomentSystem
from .inertia import *
from .RigidBodyStates import *
from .RigidBodies import *

# For some reason CythonVector and company don't exist down here, so they won't import when running from MAPLEAF.Motion import *
subModules = [ Integration, inertia, RigidBodies, RigidBodyStates, Interpolation, forceMomentSystem ]

__all__ = [ "Vector", "Quaternion", "AngularVelocity" ]

for subModule in subModules:
    __all__ += subModule.__all__

Sub-modules

MAPLEAF.Motion.AeroParameters

Defines functions to calculate the Mach and Re numbers, angles of attack, etc… Several have a standardized interface, and are used for control gain …

MAPLEAF.Motion.CythonAngularVelocity

MAPLEAF.Motion.CythonQuaternion

MAPLEAF.Motion.CythonVector

MAPLEAF.Motion.Integration

Defines ODE integrators for constant and adaptive time stepping. Used by the RigidBody classes to integrate rocket motion …

MAPLEAF.Motion.Interpolation

Interpolation rigid body states and scalar values. State interpolation used in flight animations. Scalar interpolation used for interpolation of …

MAPLEAF.Motion.RigidBodies

Classes that represent 3- and 6-DoF rigid bodies. Rigid body classes contain the logic for calculating rigid body state derivatives, given current …

MAPLEAF.Motion.RigidBodyStates

Define standard and time-derivative rigid body states.
These are defined in such a way that, for the purposes of Runge-Kutta motion integration, …

MAPLEAF.Motion.forceMomentSystem

MAPLEAF.Motion.inertia

Functions

def calculateCubicInterpCoefficients(X1, X2, Y1, Y2, dydx1, dydx2)

Returns coefficients for a cubic polynomial that matches values and derivatives at x1 and x2

Expand source code

def calculateCubicInterpCoefficients(X1, X2, Y1, Y2, dydx1, dydx2):
    ''' Returns coefficients for a cubic polynomial that matches values and derivatives at x1 and x2 '''
    # AMatrix and B, together define the following equations which constrain the cubic interpolation
    # f(x=x1)       == Y1
    # f(x=x2)       == Y2
    # df/dx (x=x1)  == dydx1
    # df/dx (x=x2)  == dydx2
    Ainv = getInvAMatrix(X1, X2)
    
    B = np.array([  [Y1], 
                    [Y2], 
                    [dydx1], 
                    [dydx2]])

    return Ainv.dot(B)

def cubicInterp(X, X1, X2, Y1, Y2, Y1_plusDx, Y2_plusDx, dx)

Expand source code

def cubicInterp(X, X1, X2, Y1, Y2, Y1_plusDx, Y2_plusDx, dx):
    dy_dx_x1 = (Y1_plusDx - Y1) / dx
    dy_dx_x2 = (Y2_plusDx - Y2) / dx

    interpCoeffs = calculateCubicInterpCoefficients(X1, X2, Y1, Y2, dy_dx_x1, dy_dx_x2)
    return float(interpCoeffs[0] + interpCoeffs[1]*X + interpCoeffs[2]*X**2 + interpCoeffs[3]*X**3)

def integratorFactory(integrationMethod='Euler', simDefinition=None, discardedTimeStepCallback=None)

Returns a callable integrator object

Inputs

• integrationMethod: (str) Name of integration method: Examples = "Euler", "RK4", "RK23Adaptive", and "RK45Adaptive"
• simDefinition: (SimDefinition) for adaptive integration, provide a simdefinition file with time step adaptation parameters
• discardedTimeStepCallback: (1-argument function reference) for adaptive integration, this function (if provided) is called when a time step is computed, but then discarded and re-computed with a smaller timestep to remain below the max estimated error threshold. Used by MAPLEAF to remove force calculation logs from those discarded time steps

Expand source code

def integratorFactory(integrationMethod="Euler", simDefinition=None, discardedTimeStepCallback=None):
    ''' 
        Returns a callable integrator object

        Inputs:
            * integrationMethod: (str) Name of integration method: Examples = "Euler", "RK4", "RK23Adaptive", and "RK45Adaptive"
            * simDefinition: (`MAPLEAF.IO.SimDefinition`) for adaptive integration, provide a simdefinition file with time step adaptation parameters
            * discardedTimeStepCallback: (1-argument function reference) for adaptive integration, this function (if provided) is called when a time step is computed,
                but then discarded and re-computed with a smaller timestep to remain below the max estimated error threshold. Used by MAPLEAF to remove
                force calculation logs from those discarded time steps
    '''
    if "Adapt" in integrationMethod:
        if simDefinition == None:
            raise ValueError("SimDefinition object required to initialize adaptive integrator")

        from MAPLEAF.IO import SubDictReader
        adaptDictReader = SubDictReader("SimControl.TimeStepAdaptation", simDefinition)

        # Adaptive Integration
        controller = adaptDictReader.getString("controller")
        if controller == "PID":
            PIDCoeffs = [ float(x) for x in adaptDictReader.getString("PID.coefficients").split() ]
            safetyFactor = None
        elif controller == "elementary":
            safetyFactor = adaptDictReader.getFloat("Elementary.safetyFactor")
            PIDCoeffs = None

        targetError = adaptDictReader.getFloat("targetError")
        minFactor = adaptDictReader.getFloat("minFactor")
        maxFactor = adaptDictReader.getFloat("maxFactor")
        maxTimeStep = adaptDictReader.getFloat("maxTimeStep")
        minTimeStep = adaptDictReader.getFloat("minTimeStep")
        
        return AdaptiveIntegrator(
            method=integrationMethod, 
            controller=controller, 
            targetError=targetError, 
            maxMinSafetyFactors=[maxFactor, minFactor, safetyFactor], 
            PIDCoeffs=PIDCoeffs, 
            maxTimeStep=maxTimeStep, 
            minTimeStep=minTimeStep, 
            discardedTimeStepCallback=discardedTimeStepCallback
        )
    
    else:
        # Constant time step integrator
        return ClassicalIntegrator(method=integrationMethod)

def interpolateRigidBodyStates(state1, state2, state1Weight)

Linearly interpolates between state 1 and state2. state1Weight should be a decimal value between 0 and 1.

Expand source code

def interpolateRigidBodyStates(state1, state2, state1Weight):
    '''
        Linearly interpolates between state 1 and state2.
        state1Weight should be a decimal value between 0 and 1.
    '''
    state2Weight = 1 - state1Weight
    
    # Properties of all rigid body states
    pos = state1.position*state1Weight + state2.position*state2Weight
    vel = state1.velocity*state1Weight + state2.velocity*state2Weight

    try:
        # 6DoF Properties
        orientationDelta = state1.orientation.slerp(state2.orientation, state1Weight) # Use spherical linear interpolation for quaternions
        orientation = state1.orientation * orientationDelta
        angVel = state1.angularVelocity*state1Weight + state2.angularVelocity*state2Weight
        return RigidBodyState(pos, vel, orientation, angVel)

    except AttributeError:
        # 3DoF doesn't include orientation / angVel
        return RigidBodyState_3DoF(pos, vel)

def linInterp(X, Y, desiredX)

Arguments

X: Sorted list or numpy array of numeric x-values Y: Sorted list or numpy array of numeric y-values desiredX: Numeric x-value, indicating point to interpolate to

Returns

desiredY

The linearly-interpolated y value at x=desiredX

Notes

Uses binary search (bisect) to locate interpolation interval Faster than built-in methods for our application (see test/LinInterpSpeed.py)

Expand source code

def linInterp(X, Y, desiredX):
    '''
        Arguments:
            X: Sorted list or numpy array of numeric x-values
            Y: Sorted list or numpy array of numeric y-values
            desiredX: Numeric x-value, indicating point to interpolate to

        Returns:
            desiredY: The linearly-interpolated y value at x=desiredX

        Notes:
            Uses binary search (bisect) to locate interpolation interval
            Faster than built-in methods for our application (see test/LinInterpSpeed.py)
    '''
    interpPt = bisect(X, desiredX)
    
    if interpPt >= len(X):
        return Y[len(X)-1]
    elif interpPt < 1:
        return Y[0]
    else:
        lgX = X[interpPt]
        smX = X[interpPt-1]
        lgY = Y[interpPt]
        smY = Y[interpPt-1]

    return (lgY - smY)*(desiredX - smX)/(lgX - smX) + smY

def linInterpWeights(X, desiredX)

Expects the list X is sorted Returns smallYIndex, smallYWeight, largeYIndex, largeYWeight: Ex: X = [ 0, 1, 2, 3 ], desiredX = 0.75 smallYIndex = 0 smallYWeight = 0.25 largeYIndex = 1 largeYWeight = 0.75

Then, to calculate the interpolate value:
    interpVal = Y[smallYIndex]*smallYWeight + Y[largeYIndex]*largeYWeight

Expand source code

def linInterpWeights(X, desiredX):
    ''' 
        Expects the list X is sorted
        Returns smallYIndex, smallYWeight, largeYIndex, largeYWeight:
            Ex: X = [ 0, 1, 2, 3 ], desiredX = 0.75
                smallYIndex = 0
                smallYWeight = 0.25
                largeYIndex = 1
                largeYWeight = 0.75

            Then, to calculate the interpolate value:
                interpVal = Y[smallYIndex]*smallYWeight + Y[largeYIndex]*largeYWeight
    '''
    interpPt = bisect(X, desiredX)

    #Edge cases
    if interpPt >= len(X):
        return 0, 0, -1, 1
    elif interpPt < 1:
        return 0, 1, 0, 0

    # Normal cases
    smallYIndex = interpPt -1
    largeYIndex = interpPt
    largeYWeight = (desiredX - X[smallYIndex]) / (X[largeYIndex] - X[smallYIndex])
    smallYWeight = 1 - largeYWeight

    return smallYIndex, smallYWeight, largeYIndex, largeYWeight

Classes

class AngularVelocity (...)

Uses a vector to represent an angular velocity

Initialize from axis-angle by passing in a Vector for axisOfRotation, and a numeric value for angularVel Can also pass in the components of a rotationVector, treating this function as init(X, Y, Z)

Ancestors

• Vector

Methods

def angVel(...)

def rotationAxis(...)

def toQuaternion(...)

After an angular velocity has been multiplied by a timestep (integrated), used to convert it to a Quaternion representing a rotation over a timestep

Inherited members

• Vector:
  • X
  • Y
  • Z
  • angle
  • normalize

class ForceMomentSystem (force, location=None, moment=None)

Defines an applied force-moment pair at a location. Can be moved to other locations - recalculates the moment accordingly.

Expand source code

class ForceMomentSystem():
    '''
        Defines an applied force-moment pair at a location.
        Can be moved to other locations - recalculates the moment accordingly.
    '''
    __slots__ = [ "force", "location", "moment" ]

    def __init__(self, force, location=None, moment=None):
        self.force = force

        # Avoids using mutable default values
        if location == None:
            location = Vector(0,0,0)
        self.location = location

        # Avoids using mutable default values
        if moment == None:
            moment = Vector(0,0,0)
        self.moment = moment

    def __add__(self, force2):
        '''
            New force/moment system is calculated about self.location
        '''
        force2AtPresentLocation = force2.getAt(self.location)
        newForce = self.force + force2AtPresentLocation.force
        newMoment = self.moment + force2AtPresentLocation.moment
        return ForceMomentSystem(newForce, self.location, newMoment)

    def __neg__(self):
        return ForceMomentSystem(-self.force, self.location, -self.moment)

    def __sub__(self, force2):
        return self + (-force2)

    def getAt(self, newLocation):
        '''
            Returns a new force object, where the application location has been changed and the applied moment has been updated accordingly

            Moving the force/moment pair application to a new application location does not change the resulting applied force
            Only the applied moment changes to compensate for the force application location change
        '''
        # Find moment that the old force produces about the new location
        newToOld = self.location - newLocation
        momentAppliedAboutNewLocation = newToOld.crossProduct(self.force)
        # Add it to the previous moment
        return ForceMomentSystem(self.force, newLocation, self.moment + momentAppliedAboutNewLocation)

    def __str__(self):
        ''' Return string representation of object (Used by print()) '''
        return 'Force=({}) At=({}) + Moment=({})'.format(self.force, self.location, self.moment)

    def __eq__(self, force2):
        return self.force == force2.force and self.location == force2.location and self.moment == force2.moment

Instance variables

var force

Return an attribute of instance, which is of type owner.

var location

Return an attribute of instance, which is of type owner.

var moment

Return an attribute of instance, which is of type owner.

Methods

def getAt(self, newLocation)

Returns a new force object, where the application location has been changed and the applied moment has been updated accordingly

Moving the force/moment pair application to a new application location does not change the resulting applied force Only the applied moment changes to compensate for the force application location change

Expand source code

def getAt(self, newLocation):
    '''
        Returns a new force object, where the application location has been changed and the applied moment has been updated accordingly

        Moving the force/moment pair application to a new application location does not change the resulting applied force
        Only the applied moment changes to compensate for the force application location change
    '''
    # Find moment that the old force produces about the new location
    newToOld = self.location - newLocation
    momentAppliedAboutNewLocation = newToOld.crossProduct(self.force)
    # Add it to the previous moment
    return ForceMomentSystem(self.force, newLocation, self.moment + momentAppliedAboutNewLocation)

class Inertia (MOI, CG, mass, MOICentroidLocation=None)

• MOIVector: Ixx, Iyy, Izz
• MOICentroidLocation: The point (in the rocket frame) about which Ixx, Iyy, and Izz were calculated. Only required if different from CG

Expand source code

class Inertia():

    ___slots__ = [ "MOI", "MOICentroidLocation", "mass", "CG" ]

    def __init__(self, MOI, CG, mass, MOICentroidLocation=None):
        """ 
            * MOIVector: Ixx, Iyy, Izz  
            * MOICentroidLocation: The point (in the rocket frame) about which Ixx, Iyy, and Izz were calculated. Only required if different from CG
        """
        self.MOI = MOI
        self.CG = CG
        self.mass = mass
        self.MOICentroidLocation = CG if (MOICentroidLocation == None) else MOICentroidLocation

    def checkDefinedAboutCG(self, inertiasList):
        '''
            Called by the functions that add/combine inertias.
            Checks that every inertia to be added up has MOIs defined about its CG.
            Throws a ValueError if this is not the case
        '''
        for inertia in inertiasList:
            if inertia.MOICentroidLocation != inertia.CG:
                raise ValueError("At least one of the inertias to be added has an MOI not defined about its CG: {}".format(inertia))

    def combineInertiasAboutPoint(self, inertiasList, Point):
        """
            Version of the combineInertias function below, which can combine Inertias about a point which is not coincident with the combined CG of the resulting combined object.
            This changes the resulting moments of inertia.

            Inputs:
                inertiasList:   List of other inertia objects. Can also be empty list, then parallel axis theorem is just applied to the present inertia object
                Point:          Point about which summed inertias will be calculated
            Returns:
                New instance of Inertia, with MOI defined about Point
        """    
        # Initialize variables to hold results
        totalMass = 0
        totalCG = Vector(0,0,0)
        totalMOI = Vector(0,0,0)

        # Add the current object to the list of Inertia objects to be combined
        if self not in inertiasList:
            inertiasList.append(self)

        self.checkDefinedAboutCG(inertiasList)

        # Add up all the inertias in the list
        for inertia in inertiasList:
            # Add mass and mass-weighted CG
            totalMass += inertia.mass
            totalCG += inertia.CG * inertia.mass

            ### Add inertia using parallel axis theorem ###
            
            # Add current inertia
            totalMOI += inertia.MOI
            # Add adjustment for different axis location
            distanceFromPoint = Point - inertia.MOICentroidLocation
            xAxesDistSqr = distanceFromPoint.Y*distanceFromPoint.Y + distanceFromPoint.Z*distanceFromPoint.Z
            yAxesDistSqr = distanceFromPoint.X*distanceFromPoint.X + distanceFromPoint.Z*distanceFromPoint.Z
            zAxesDistSqr = distanceFromPoint.X*distanceFromPoint.X + distanceFromPoint.Y*distanceFromPoint.Y
            totalMOI.X += inertia.mass*xAxesDistSqr
            totalMOI.Y += inertia.mass*yAxesDistSqr
            totalMOI.Z += inertia.mass*zAxesDistSqr

        # Compute actual CG by dividing out the mass
        totalCG = totalCG / totalMass

        return Inertia(totalMOI, totalCG, totalMass, MOICentroidLocation=Point)

    def combineInertias(self, inertiasList):
        """
            Combines inertias about the CG of the combined object
            Each component inertia must be defined about the component's CG
        """
        # Add the current object to the list of Inertia objects to be combined
        if self not in inertiasList:
            inertiasList.append(self)

        self.checkDefinedAboutCG(inertiasList)

        # Calculate combined CG
        totalMass = 0
        totalCG = Vector(0,0,0)

        for inertia in inertiasList:
            totalMass += inertia.mass
            totalCG += inertia.CG * inertia.mass # Adding mass-weighted CG

        if totalMass > 0:
            totalCG = totalCG / totalMass # Compute actual CG by dividing out the total mass
        else:
            totalCG = Vector(0,0,0)

        # Calculate combined MOI about combined CG
        totalMOI = Vector(0,0,0)
        for inertia in inertiasList:
            totalMOI += inertia.MOI # Add current inertia
            # Add adjustment for different axis location - parallel axis theorem
            distanceFromPoint = totalCG - inertia.MOICentroidLocation
            xAxesDistSqr = distanceFromPoint.Y*distanceFromPoint.Y + distanceFromPoint.Z*distanceFromPoint.Z
            yAxesDistSqr = distanceFromPoint.X*distanceFromPoint.X + distanceFromPoint.Z*distanceFromPoint.Z
            zAxesDistSqr = distanceFromPoint.X*distanceFromPoint.X + distanceFromPoint.Y*distanceFromPoint.Y
            totalMOI.X += inertia.mass*xAxesDistSqr
            totalMOI.Y += inertia.mass*yAxesDistSqr
            totalMOI.Z += inertia.mass*zAxesDistSqr

        return Inertia(totalMOI, totalCG, totalMass)

    def __add__(self, inertia2):
        return self.combineInertias([inertia2])

    def __str__(self):
        ''' Get string representation, used by str() and print() '''
        if self.MOICentroidLocation == self.CG:
            return 'MOI=({}) calculated about CG=({}); Mass={} '.format(self.MOI, self.CG, self.mass)
        else:
            return 'MOI=({}) calculated about non-CG Point=({}); CG=({}); Mass={}'.format(self.MOI, self.MOICentroidLocation, self.CG, self.mass)

    def __eq__(self, inertia2):
        try:
            c1 = self.MOI == inertia2.MOI
            c2 = self.MOICentroidLocation == inertia2.MOICentroidLocation
            c3 = self.mass == inertia2.mass
            c4 = self.CG == inertia2.CG
            if c1 and c2 and c3 and c4:
                return True
            else:
                return False

        except AttributeError:
            return False

Methods

def checkDefinedAboutCG(self, inertiasList)

Called by the functions that add/combine inertias. Checks that every inertia to be added up has MOIs defined about its CG. Throws a ValueError if this is not the case

Expand source code

def checkDefinedAboutCG(self, inertiasList):
    '''
        Called by the functions that add/combine inertias.
        Checks that every inertia to be added up has MOIs defined about its CG.
        Throws a ValueError if this is not the case
    '''
    for inertia in inertiasList:
        if inertia.MOICentroidLocation != inertia.CG:
            raise ValueError("At least one of the inertias to be added has an MOI not defined about its CG: {}".format(inertia))

def combineInertias(self, inertiasList)

Combines inertias about the CG of the combined object Each component inertia must be defined about the component's CG

Expand source code

def combineInertias(self, inertiasList):
    """
        Combines inertias about the CG of the combined object
        Each component inertia must be defined about the component's CG
    """
    # Add the current object to the list of Inertia objects to be combined
    if self not in inertiasList:
        inertiasList.append(self)

    self.checkDefinedAboutCG(inertiasList)

    # Calculate combined CG
    totalMass = 0
    totalCG = Vector(0,0,0)

    for inertia in inertiasList:
        totalMass += inertia.mass
        totalCG += inertia.CG * inertia.mass # Adding mass-weighted CG

    if totalMass > 0:
        totalCG = totalCG / totalMass # Compute actual CG by dividing out the total mass
    else:
        totalCG = Vector(0,0,0)

    # Calculate combined MOI about combined CG
    totalMOI = Vector(0,0,0)
    for inertia in inertiasList:
        totalMOI += inertia.MOI # Add current inertia
        # Add adjustment for different axis location - parallel axis theorem
        distanceFromPoint = totalCG - inertia.MOICentroidLocation
        xAxesDistSqr = distanceFromPoint.Y*distanceFromPoint.Y + distanceFromPoint.Z*distanceFromPoint.Z
        yAxesDistSqr = distanceFromPoint.X*distanceFromPoint.X + distanceFromPoint.Z*distanceFromPoint.Z
        zAxesDistSqr = distanceFromPoint.X*distanceFromPoint.X + distanceFromPoint.Y*distanceFromPoint.Y
        totalMOI.X += inertia.mass*xAxesDistSqr
        totalMOI.Y += inertia.mass*yAxesDistSqr
        totalMOI.Z += inertia.mass*zAxesDistSqr

    return Inertia(totalMOI, totalCG, totalMass)

def combineInertiasAboutPoint(self, inertiasList, Point)

Version of the combineInertias function below, which can combine Inertias about a point which is not coincident with the combined CG of the resulting combined object. This changes the resulting moments of inertia.

Inputs

inertiasList: List of other inertia objects. Can also be empty list, then parallel axis theorem is just applied to the present inertia object Point: Point about which summed inertias will be calculated

Returns

New instance of Inertia, with MOI defined about Point

Expand source code

def combineInertiasAboutPoint(self, inertiasList, Point):
    """
        Version of the combineInertias function below, which can combine Inertias about a point which is not coincident with the combined CG of the resulting combined object.
        This changes the resulting moments of inertia.

        Inputs:
            inertiasList:   List of other inertia objects. Can also be empty list, then parallel axis theorem is just applied to the present inertia object
            Point:          Point about which summed inertias will be calculated
        Returns:
            New instance of Inertia, with MOI defined about Point
    """    
    # Initialize variables to hold results
    totalMass = 0
    totalCG = Vector(0,0,0)
    totalMOI = Vector(0,0,0)

    # Add the current object to the list of Inertia objects to be combined
    if self not in inertiasList:
        inertiasList.append(self)

    self.checkDefinedAboutCG(inertiasList)

    # Add up all the inertias in the list
    for inertia in inertiasList:
        # Add mass and mass-weighted CG
        totalMass += inertia.mass
        totalCG += inertia.CG * inertia.mass

        ### Add inertia using parallel axis theorem ###
        
        # Add current inertia
        totalMOI += inertia.MOI
        # Add adjustment for different axis location
        distanceFromPoint = Point - inertia.MOICentroidLocation
        xAxesDistSqr = distanceFromPoint.Y*distanceFromPoint.Y + distanceFromPoint.Z*distanceFromPoint.Z
        yAxesDistSqr = distanceFromPoint.X*distanceFromPoint.X + distanceFromPoint.Z*distanceFromPoint.Z
        zAxesDistSqr = distanceFromPoint.X*distanceFromPoint.X + distanceFromPoint.Y*distanceFromPoint.Y
        totalMOI.X += inertia.mass*xAxesDistSqr
        totalMOI.Y += inertia.mass*yAxesDistSqr
        totalMOI.Z += inertia.mass*zAxesDistSqr

    # Compute actual CG by dividing out the mass
    totalCG = totalCG / totalMass

    return Inertia(totalMOI, totalCG, totalMass, MOICentroidLocation=Point)

class NoNaNLinearNDInterpolator (keys, values, tablePath=None)

Expand source code

class NoNaNLinearNDInterpolator():
    def __init__(self, keys, values, tablePath=None) -> None:
        self.linearInterpolator = LinearNDInterpolator(keys, values)
        self.nearestInterpolator = NearestNDInterpolator(keys, values)
        self.tablePath = tablePath

    def __call__(self, *keyVector):
        linearResult = self.linearInterpolator(*keyVector)
        
        if np.isnan(linearResult).any():
            # Occurs if the requested values are outside of the bounds of the table being interpolated over
                # In that case just return the nearest result
            print("WARNING: Interpolation requested outside of bounds in table: {}. Current key vector = {}. Extrapolation not supported, returning nearest result instead".format(self.tablePath, keyVector))
            return self.nearestInterpolator(*keyVector)
        
        else:
            return linearResult  

class Quaternion (...)

Use objects of this class to store orientation information - direction and rotation Elements are: [ scalar, xi, yj, zk ] where scalar, x, y, and z are scalars i,j, and k squared = -1 ij=k ji=-k jk=i kj=-i ki=j ik=-j

To define the rotation quaternion between two frames: If Q(2) = Q(1) * Q(1-2) Q(1)^-1 * Q(2) = Q(1)^-1 * Q(1) * Q(1-2) Q(1)^-1 * Q(2) = Q(1-2) ------------- where Q(1-2) defines the rotation from frame 1 to 2 (as defined in the first step) Q(2-1) = Q(1-2).conjugate

Quaternion normalization is not enforced by this class, but quaternions do not represent a rotation unless they are normalized. Quaternions are automatically normalized if this class is constructed using the angle/axis terms and if .rotate() is used.

Future: Could increase rotation performance by ~50% by converting to a rotation matrix Only worth it if we rotate many vectors per quaternion

A positive angle of rotation is clockwise if we are looking down the defined rotation axis

Class definition in CythonQuaternion.pxd

Can initialize in one of three ways: 1. orientation = Quaternion(axisOfRotation=Vector(0,0,1), angle=(pi)) - angle should be in radians 2. orientation = Quaternion(components=[0,1,0,1]) NOTE: Deprecated - will be removed 3. orientation = Quaternion(0, 1, 0, 1) - Fastest approach

Instance variables

var Q0

Return an attribute of instance, which is of type owner.

var Q1

Return an attribute of instance, which is of type owner.

var Q2

Return an attribute of instance, which is of type owner.

var Q3

Return an attribute of instance, which is of type owner.

Methods

def conjugate(...)

Negate all complex terms - produces an inverse rotation if the quaternion is a unit quaternion

def dotProduct(...)

Used to compute the angle parameter in spherical linear interpolation

def inverse(...)

Conjugate divided by self.norm

def norm(...)

Get the norm of the quaternion

def normalize(...)

Create unit quaternion

def plotRotation(...)

def rotate(...)

Use the quaternion to rotate a vector

def rotationAngle(...)

Returns the angle (radians) by which this quaternion rotates vectors about the rotationAxis

def rotationAxis(...)

Returns the axis about which this quaternion would rotate a vector

def scaleRotation(...)

def slerp(...)

Spherical Linear Interpolation - fraction = (0-1) where 0 is self, 1 is quat2

def toEulerAngles(...)

Returns Tait-Bryan 3-2-1, z-y-x convention Euler Angles Vector

def toRotationVector(...)

class RigidBody (rigidBodyState, forceParam, inertiaParam, integrationMethod='Euler', discardedTimeStepCallback=None, simDefinition=None)

6DoF version of RigidBody_3DoF. Calculates angular velocities and accelerations

Interface

Properties: .state = RigidBodyState (pos, vel, orientation, angVel) .time = currentSimTime

External functions wrapped by this class .forceFunc(time, state) .inertiaFunc(time, state)

Methods .timeStep(deltaT)

Just calls RigidBodyState_3DoF constructor

Expand source code

class RigidBody(RigidBody_3DoF):
    """
        6DoF version of RigidBody_3DoF. Calculates angular velocities and accelerations

        Interface:
            Properties:
                .state = RigidBodyState (pos, vel, orientation, angVel)
                .time = currentSimTime
            
            External functions wrapped by this class
                .forceFunc(time, state)
                .inertiaFunc(time, state)

            Methods
                .timeStep(deltaT)
    """
    #TODO: Moment of Inertia Tensor? Required for aircraft

    def __init__(self, rigidBodyState, forceParam, inertiaParam, integrationMethod="Euler", discardedTimeStepCallback=None, simDefinition=None):
        ''' Just calls RigidBodyState_3DoF constructor '''
        super().__init__(rigidBodyState, forceParam, inertiaParam, integrationMethod=integrationMethod, simDefinition=simDefinition, discardedTimeStepCallback=discardedTimeStepCallback)
        self.lastStateDerivative = RigidBodyStateDerivative(rigidBodyState.velocity, Vector(0,0,0), rigidBodyState.angularVelocity, AngularVelocity(0,0,0))

    def getRigidBodyStateDerivative(self, time, state):
        # Forces are expected to be calculated in a body frame, where each coordinate axis is aligned with a pricipal axis
        appliedForce_localFrame = self.forceFunc(time, state)
        inertia = self.inertiaFunc(time, state)

        # Get CG velocity relative to body geometry
        dt = 1e-8
        inertia2 = self.inertiaFunc(time+dt, state)
        CGVel_local = (inertia2.CG - inertia.CG) / dt
        CGVel_global = state.orientation.rotate(CGVel_local)

        ### Translation - calculated in global frame ###
        #Convert from local to global frame
        fVec_global = state.orientation.rotate(appliedForce_localFrame.force)        
        linAccel_global = fVec_global / inertia.mass

        ### Rotation - calculated in local frame (Euler equations) ###
        # convert angular velocity to global frame - to be added to orientation
        angVel_global = AngularVelocity(*state.orientation.rotate(state.angularVelocity)) # Will be transformed into a quaternion once multiplied by a timestep
        
        # Calc angular acceleration (in local frame)
        moi = inertia.MOI
        dAngVelDtX = (appliedForce_localFrame.moment.X + (moi.Y - moi.Z) * state.angularVelocity.Y * state.angularVelocity.Z) / moi.X
        dAngVelDtY = (appliedForce_localFrame.moment.Y + (moi.Z - moi.X) * state.angularVelocity.Z * state.angularVelocity.X) / moi.Y
        dAngVelDtZ = (appliedForce_localFrame.moment.Z + (moi.X - moi.Y) * state.angularVelocity.X * state.angularVelocity.Y) / moi.Z
        dAngVelDt = AngularVelocity(dAngVelDtX, dAngVelDtY, dAngVelDtZ)

        return RigidBodyStateDerivative(state.velocity+CGVel_global, linAccel_global, angVel_global, dAngVelDt)

Ancestors

• RigidBody_3DoF

Subclasses

• StatefulRigidBody

Methods

def getRigidBodyStateDerivative(self, time, state)

Expand source code

def getRigidBodyStateDerivative(self, time, state):
    # Forces are expected to be calculated in a body frame, where each coordinate axis is aligned with a pricipal axis
    appliedForce_localFrame = self.forceFunc(time, state)
    inertia = self.inertiaFunc(time, state)

    # Get CG velocity relative to body geometry
    dt = 1e-8
    inertia2 = self.inertiaFunc(time+dt, state)
    CGVel_local = (inertia2.CG - inertia.CG) / dt
    CGVel_global = state.orientation.rotate(CGVel_local)

    ### Translation - calculated in global frame ###
    #Convert from local to global frame
    fVec_global = state.orientation.rotate(appliedForce_localFrame.force)        
    linAccel_global = fVec_global / inertia.mass

    ### Rotation - calculated in local frame (Euler equations) ###
    # convert angular velocity to global frame - to be added to orientation
    angVel_global = AngularVelocity(*state.orientation.rotate(state.angularVelocity)) # Will be transformed into a quaternion once multiplied by a timestep
    
    # Calc angular acceleration (in local frame)
    moi = inertia.MOI
    dAngVelDtX = (appliedForce_localFrame.moment.X + (moi.Y - moi.Z) * state.angularVelocity.Y * state.angularVelocity.Z) / moi.X
    dAngVelDtY = (appliedForce_localFrame.moment.Y + (moi.Z - moi.X) * state.angularVelocity.Z * state.angularVelocity.X) / moi.Y
    dAngVelDtZ = (appliedForce_localFrame.moment.Z + (moi.X - moi.Y) * state.angularVelocity.X * state.angularVelocity.Y) / moi.Z
    dAngVelDt = AngularVelocity(dAngVelDtX, dAngVelDtY, dAngVelDtZ)

    return RigidBodyStateDerivative(state.velocity+CGVel_global, linAccel_global, angVel_global, dAngVelDt)

class RigidBodyState (position=None, velocity=None, orientation=None, angularVelocity=None)

Class created to be able to treat rigidBody states like scalars when integrating the movement of a rigid body Pos/Vel are expected to be Vectors - Defined with reference to the global frame Orientation is expected to be a Quaternion - Defines the rotation from the global inertial reference frame to the rocket's local frame (Orientation of the rocket in the global frame) Angular Velocity is expected to be an Angular Velocity - Defined with reference to the local frame

Adding rigidBodyStates adds the vectors and multiplies the quaternions (which adds the rotations they represent)

Multiplying an rigidBodyState by a scalar scales the vectors and rotation defined by the quaternions 0.5 * = half the vector length, half the rotation size, directions the same

No other operations are defined

Expand source code

class RigidBodyState():
    """ Class created to be able to treat rigidBody states like scalars when integrating the movement of a rigid body
            Pos/Vel are expected to be Vectors - Defined with reference to the global frame
            Orientation is expected to be a Quaternion - Defines the rotation from the global inertial reference frame to the rocket's local frame 
                (Orientation of the rocket in the global frame)
            Angular Velocity is expected to be an Angular Velocity - Defined with reference to the local frame

        Adding rigidBodyStates adds the vectors and multiplies the quaternions (which adds the rotations they represent)

        Multiplying an rigidBodyState by a scalar scales the vectors and rotation defined by the quaternions
            0.5 *  = half the vector length, half the rotation size, directions the same

        No other operations are defined
    """
    def __init__(self, position=None, velocity=None, orientation=None, angularVelocity=None):
        self.position = Vector(0,0,0) if (position is None) else position
        self.velocity = Vector(0,0,0) if (velocity is None) else velocity
        self.orientation = Quaternion(1,0,0,0) if (orientation is None) else orientation
        self.angularVelocity = AngularVelocity(0,0,0) if (angularVelocity is None) else angularVelocity
    
    def __add__(self, rigidBodyState2):
        ''' Used in: initVal {+} (timeStep * slope) '''
        newPos = self.position + rigidBodyState2.position
        newVel = self.velocity + rigidBodyState2.velocity
        newAngVel = self.angularVelocity + rigidBodyState2.angularVelocity
        newOrientation = rigidBodyState2.orientation * self.orientation.normalize()
        
        return RigidBodyState(newPos, newVel, newOrientation.normalize(), newAngVel)

    def __mul__(self, timeStep):
        '''
            Used in: initVal + timeStep {*} slope
            Expected to always be multiplied by a scalar
        '''
        timeStep = float(timeStep)

        newPos = self.position * timeStep
        newVel = self.velocity * timeStep
        newAngVel = self.angularVelocity * timeStep
        newOrientation = self.orientation.scaleRotation(timeStep)

        return RigidBodyState(newPos, newVel, newOrientation, newAngVel)

    def __abs__(self):
        ''' Used to quantify the difference between two RigidBodyStates as a scalar value during error estimation in adaptive time stepping methods '''

        positionMag = self.position.length() + self.velocity.length()
        orientationMag = abs(self.orientation.rotationAngle()) + self.angularVelocity.angVel()

        return orientationMag*100 + positionMag

    def __eq__(self, iRBS2):
        try:
            properties = [ self.position, self.velocity, self.orientation, self.angularVelocity ]
            otherProperties = [ iRBS2.position, iRBS2.velocity, iRBS2.orientation, iRBS2.angularVelocity ]
            return all([ x == y for (x,y) in zip(properties, otherProperties) ])
        except AttributeError:
            return False

    def __str__(self):
        ''' Called by print function '''
        return " {:>10.3f} {:>10.4f} {:>11.7f} {:>9.4f}".format(self.position, self.velocity, self.orientation, self.angularVelocity)

    ### Wrapper/Thin functions ###
    def __neg__(self):
        return RigidBodyState(self.position*-1, self.velocity*-1, self.orientation.conjugate(), self.angularVelocity*-1)

class RigidBodyStateDerivative (velocity, acceleration, angularVelocity, angularAccel)

Expand source code

class RigidBodyStateDerivative():
    def __init__(self, velocity, acceleration, angularVelocity, angularAccel):
        self.velocity = velocity
        self.acceleration = acceleration
        self.angularVelocity = angularVelocity
        self.angularAccel = angularAccel

    def __add__(self, state2):
        ''' Used in: initVal {+} (timeStep * slope) '''
        newVel = self.velocity + state2.velocity
        newAccel = self.acceleration + state2.acceleration
        newAngVel = self.angularVelocity + state2.angularVelocity
        newAngAccel = self.angularAccel + state2.angularAccel
        
        return RigidBodyStateDerivative(newVel, newAccel, newAngVel, newAngAccel)

    def __mul__(self, timeStep):
        '''
            Used in: timeStep {*} slope
            Expected to always be multiplied by a scalar, timestep
            Returns integrated change in rigid body state over given time step
        '''
        timeStep = float(timeStep)

        dPos = self.velocity * timeStep
        dVel = self.acceleration * timeStep
        dOrientation = (self.angularVelocity * timeStep).toQuaternion()
        dOrientation.normalize()
        dAngVel = self.angularAccel * timeStep

        # After integration over a time step, get a regular rigid body state back
        return RigidBodyState(dPos, dVel, dOrientation, dAngVel)

    def __truediv__(self, invScalar):
        ''' 
            Used in (k1 + k4) {/} 2 
            Does not 'integrate' the result to make a rigidBodyState, returns a new rigidBodyStateDerivative
        '''
        invScalar = 1/float(invScalar)

        vel = self.velocity * invScalar
        accel = self.acceleration * invScalar
        angVel = self.angularVelocity * invScalar
        angAccel = self.angularAccel * invScalar

        return RigidBodyStateDerivative(vel, accel, angVel, angAccel)

    def __abs__(self):
        ''' Used to quantify the difference between two RigidBodyStates as a scalar value during error estimation in adaptive time stepping methods '''

        # positionMag = self.position.length() + self.velocity.length()

        orientationMag = self.angularVelocity.angVel() + self.angularAccel.angVel()

        return orientationMag*100

    def __eq__(self, state2):
        try:
            properties = [ self.velocity, self.acceleration, self.angularVelocity, self.angularAccel ]
            otherProperties = [ state2.velocity, state2.acceleration, state2.angularVelocity, state2.angularAccel ]
            return all([ x == y for (x,y) in zip(properties, otherProperties) ])
        except AttributeError:
            return False

    ### Wrapper/Thin functions ###
    def __rmul__(self, scalar):
        return self * scalar

class RigidBodyStateDerivative_3DoF (velocity, acceleration)

Expand source code

class RigidBodyStateDerivative_3DoF():
    def __init__(self, velocity, acceleration):
        self.velocity = velocity
        self.acceleration = acceleration

    def __add__(self, rigidBodyState2):
        ''' Used in: initVal {+} timeStep * slope '''
        newPos = self.velocity + rigidBodyState2.velocity
        newVel = self.acceleration + rigidBodyState2.acceleration
        
        return RigidBodyStateDerivative_3DoF(newPos, newVel)

    def __mul__(self, timeStep):
        ''' Used in: initVal + timeStep {*} slope '''
        timeStep = float(timeStep)
        dPos = self.velocity * timeStep
        dVel = self.acceleration * timeStep

        # After multiplying by a timestep, we get a regular (integrated) rigid body state back
        return RigidBodyState_3DoF(dPos, dVel)

    def __truediv__(self, invScalar):
        ''' 
            Used in (k1 + k4) {/} 2 
            Does not 'integrate' the result to make a rigidBodyState, returns a new rigidBodyStateDerivative
        '''
        invScalar = 1/float(invScalar)

        vel = self.velocity * invScalar
        accel = self.acceleration * invScalar

        return RigidBodyStateDerivative_3DoF(vel, accel)

    ### Wrapper/Thin functions ###
    def __rmul__(self, scalar):
        return self * scalar

class RigidBodyState_3DoF (position, velocity)

Class created to be able to treat rigidBody states like scalars when integrating the movement of a rigid body Pos/Vel are expected to be Vectors - Defined with reference to the global frame

Adding rigidBodyStates adds the vectors

Multiplying an rigidBodyState by a scalar scales the vectors 0.5 * = half the vector length

Expand source code

class RigidBodyState_3DoF():
    """ Class created to be able to treat rigidBody states like scalars when integrating the movement of a rigid body
            Pos/Vel are expected to be Vectors - Defined with reference to the global frame

        Adding rigidBodyStates adds the vectors

        Multiplying an rigidBodyState by a scalar scales the vectors
            0.5 *  = half the vector length
    """
    def __init__(self, position, velocity):
        self.position = position
        self.velocity = velocity

    def __add__(self, rigidBodyState2):
        ''' Used in: initVal {+} timeStep * slope '''
        newPos = self.position + rigidBodyState2.position
        newVel = self.velocity + rigidBodyState2.velocity
        
        return RigidBodyState_3DoF(newPos, newVel)

    def __mul__(self, scalar):
        ''' Used to negate the rigid body state for subtractions '''
        scalar = float(scalar)
        newPos = self.position * scalar
        newVel = self.velocity * scalar

        return RigidBodyState_3DoF(newPos, newVel)

    def __abs__(self):
        ''' Used to quantify the difference between two RigidBodyStates as a scalar value during error estimation in adaptive time stepping methods '''
        #TODO: Ensure this scales properly with the version in the 6DoF IntegratbleRigidBodyState __abs__ function

        return self.position.length() + self.velocity.length()

    def __str__(self):
        ''' Called by print '''
        return " {:>10.3f} {:>10.4f}".format(self.position, self.velocity)

    def __neg__(self):
        return self*-1

class RigidBody_3DoF (rigidBodyState, forceParam, inertiaParam, startTime=0, integrationMethod='Euler', discardedTimeStepCallback=None, simDefinition=None)

Interface:

Properties

.state = RigidBodyState (pos, vel, orientation, angVel) .time = currentSimTime

Methods

.timeStep(deltaT) -> advances simulation by deltaT

Expand source code

class RigidBody_3DoF:
    """Interface:
        ### Properties ###
        .state = RigidBodyState (pos, vel, orientation, angVel)
        .time = currentSimTime
        
        ### Methods ###
        .timeStep(deltaT) -> advances simulation by deltaT
    """
    
    ''' forceParam and inertiaParam should be functions, they will be passed:
            1) Simulation Time
            2) RigidBodyState object containing the rigidBody's Position, Velocity, Orientation and AngularVelocity
        Functions are expected to return:
            1) forceParam: A Force object containing the total force and moment applied to the rigidBody at it's CG location in the local frame of reference
            2) inertiaParam: An Inertia object containing the mass of the object (other fields ignored)
                If the rigid body is representing a CompositeObject, pass in a reference to the getMass function here
    '''
    def __init__(self, rigidBodyState, forceParam, inertiaParam, startTime=0, integrationMethod="Euler", discardedTimeStepCallback=None, simDefinition=None):
        self.time = startTime
        self.state = rigidBodyState
        
        self.forceFunc = forceParam
        self.inertiaFunc = inertiaParam
          
        self.integrate = integratorFactory(integrationMethod=integrationMethod, simDefinition=simDefinition, discardedTimeStepCallback=discardedTimeStepCallback)

        self.lastStateDerivative = RigidBodyStateDerivative_3DoF(rigidBodyState.velocity, Vector(0,0,0))

    def getRigidBodyStateDerivative(self, time, state):
        force = self.forceFunc(time, state).force
        
        DposDt = state.velocity
        DvelDt = force / self.inertiaFunc(time, state)

        return RigidBodyStateDerivative_3DoF(DposDt, DvelDt)

    def timeStep(self, deltaT):
        # TODO: Current derivative estimates are first-order, replace with a more accurate iterative method
        self.state.estimatedDerivative = self.lastStateDerivative

        integrationResult = self.integrate(self.state, self.time, self.getRigidBodyStateDerivative, deltaT)

        # This is where the simulation time and state are kept track of
        self.time += integrationResult.dt
        self.state = integrationResult.newValue

        # Save for next time step
        self.lastStateDerivative = integrationResult.derivativeEstimate

        return integrationResult

Subclasses

• RigidBody

Methods

def getRigidBodyStateDerivative(self, time, state)

Expand source code

def getRigidBodyStateDerivative(self, time, state):
    force = self.forceFunc(time, state).force
    
    DposDt = state.velocity
    DvelDt = force / self.inertiaFunc(time, state)

    return RigidBodyStateDerivative_3DoF(DposDt, DvelDt)

def timeStep(self, deltaT)

Expand source code

def timeStep(self, deltaT):
    # TODO: Current derivative estimates are first-order, replace with a more accurate iterative method
    self.state.estimatedDerivative = self.lastStateDerivative

    integrationResult = self.integrate(self.state, self.time, self.getRigidBodyStateDerivative, deltaT)

    # This is where the simulation time and state are kept track of
    self.time += integrationResult.dt
    self.state = integrationResult.newValue

    # Save for next time step
    self.lastStateDerivative = integrationResult.derivativeEstimate

    return integrationResult

class StateList (stateVariables, variableNames=None)

Purpose is to provide a generalized version of RigidBodyState, which can be integrated like a scalar, but allows MAPLEAF to integrate arbitrary additional parameters.

Contains a list of parameters that define the (rocket) state, all of which are to be integrated Overrides operators to make +-* operations elementwise Capable of representing a StateList and the derivative of a StateList

Called StateList instead of StateVector because it can contain objects of any type, like a Python list, and unlike a Vector.

..note: If we want to compile this object w/ Cython in the future, We may want to restrict ourselves to scalars

Example: RocketState([ initRigidBodyState, tankLevel1, actuatorPosition1 ])

stateVariables: (list) of state variable values (RigidBodyState assumed to be in position 0) variableNames: (list[str]) of state variable names. If passed in, can access values as attributes: stateList.varName _ nameToIndexMap:(dict[str:int]) maps variable names to indices in the itemList. If not provided, but a nameList is, will be generated from the nameList

Pass in a nameList to be able to access variables by name - order must match that of the itemList. For unnamed variables, put None in the namelist

Only pass in one of nameList or _nameToIndexMap. When both are passed in, _nameToIndexMap takes precedence. _nameToIndexMap intended for internal use only, no correctness checks performed on it

Expand source code

class StateList(list):
    ''' 
        Purpose is to provide a generalized version of RigidBodyState, which can be integrated like a scalar, but allows MAPLEAF to integrate arbitrary additional parameters.

        Contains a list of parameters that define the (rocket) state, all of which are to be integrated
        Overrides operators to make +-* operations elementwise
        Capable of representing a StateList and the derivative of a StateList

        Called StateList instead of StateVector because it can contain objects of any type, like a Python list, and unlike a Vector.
                
        ..note: If we want to compile this object w/ Cython in the future, We may want to restrict ourselves to scalars

        Example: RocketState([ initRigidBodyState, tankLevel1, actuatorPosition1 ])
    '''
    #### Initialization and setting/getting attributes ####
    def __init__(self, stateVariables, variableNames=None, _nameToIndexMap=None):
        '''
            stateVariables: (`list`) of state variable values (RigidBodyState assumed to be in position 0)
            variableNames:  (`list[str]`) of state variable names. If passed in, can access values as attributes: stateList.varName
           _ nameToIndexMap:(`dict[str:int]`) maps variable names to indices in the itemList. If not provided, but a nameList is, will be generated from the nameList

            Pass in a nameList to be able to access variables by name - order must match that of the itemList.
                For unnamed variables, put None in the namelist

            Only pass in one of nameList or _nameToIndexMap. When both are passed in, _nameToIndexMap takes precedence.
                _nameToIndexMap intended for internal use only, no correctness checks performed on it
        '''
        super().__init__(stateVariables) # Pass itemList to parent (list) constructor

        if _nameToIndexMap != None:
            # Does not check whether each variable is in the nameToIndexMap - could omit some if desired
            self.nameToIndexMap = _nameToIndexMap

        elif variableNames != None:
            # Generate nameToIndexMap from variableNames list
            if len(variableNames) == len(stateVariables):
                forbiddenVarNames = [ "position", "velocity", "orientation", "angularVelocity" ]
                for forbiddenName in forbiddenVarNames:
                    if forbiddenName in variableNames:
                        raise ValueError("ERROR: The following variable names are reserved for rigid body states: {}".format(forbiddenVarNames))

                self.nameToIndexMap = { variableNames[i]:i for i in range(len(variableNames)) }
                if len(variableNames) != len(self.nameToIndexMap.keys()):
                    raise ValueError("ERROR: Duplicate state variable name in: {}".format(variableNames))
            else:
                raise ValueError("ERROR: Number of state variables must match number of variable names provided")
        
        else:
            self.nameToIndexMap = dict()

    def __getattr__(self, name):
        try:
            # Check if the attribute name is in the nameToIndexMap - return item from that index
            return self[self.nameToIndexMap[name]]
        except KeyError:
            # Try getting the attribute from the rigidBodyState (assumed first element)
            return getattr(self[0], name)

    def __setattr__(self, name, value):
        if name in self.__dict__ or name == "nameToIndexMap":
            self.__dict__[name] = value
        elif name in self.nameToIndexMap:
            # Check if the attribute name is in the nameToIndexMap - set item at that index
            indexToSet = self.nameToIndexMap[name]
            self[indexToSet] = value
        elif name in self[0].__dict__:
            # Try getting the attribute from the rigidBodyState (assumed first element)
            setattr(self[0], name, value)

    def addStateVariable(self, name, currentValue):
        self.append(currentValue)
        self.nameToIndexMap[name] = len(self)-1

    #### Arithmetic ####
    def __add__(self, state2):
        return StateList([ x + y for x, y in zip(self, state2) ], _nameToIndexMap=self.nameToIndexMap)

    def __sub__(self, state2):
        return StateList([ x - y for x, y in zip(self, state2) ], _nameToIndexMap=self.nameToIndexMap)

    def __mul__(self, scalar):
        return StateList([ x*scalar for x in self ], _nameToIndexMap=self.nameToIndexMap)

    def __truediv__(self, scalar):
        return StateList([ x/scalar for x in self ], _nameToIndexMap=self.nameToIndexMap)

    def __rmul__(self, scalar):
        return self * scalar # Call regular __mul__ function

    def __abs__(self):
        return sum([ abs(x) for x in self ])

    def __eq__(self, state2):
        try:
            return all([ x == y for x,y in zip(self, state2) ])
        except TypeError:
            return False

    def __neg__(self):
        return StateList([ -x for x in self ], _nameToIndexMap=self.nameToIndexMap)

    #### String functions ####
    # TODO: Convert to new logging framework
    def getLogHeader(self):
        header = ""
        for i in range(len(self)):
            try:
                # If the variable defines a getLogHeader function, use it
                header += self[i].getLogHeader()
            
            except AttributeError:
                # Item doesn't have a getLogHeader function (ex. it's a float)
                # Try to find it in nameToIndexMap
                varName = None
                for key, val in self.nameToIndexMap.items():
                    if val == i:
                        varName = " " + key
                
                # Otherwise just call it stateVariableN
                if varName == None:
                    varName = " StateVariable{}".format(i)

                header += varName
        
        return header

    def __str__(self):
        varStrings = [ x.__str__() for x in self ]
        return " ".join(varStrings)

Ancestors

• builtins.list

Methods

def addStateVariable(self, name, currentValue)

Expand source code

def addStateVariable(self, name, currentValue):
    self.append(currentValue)
    self.nameToIndexMap[name] = len(self)-1

def getLogHeader(self)

Expand source code

def getLogHeader(self):
    header = ""
    for i in range(len(self)):
        try:
            # If the variable defines a getLogHeader function, use it
            header += self[i].getLogHeader()
        
        except AttributeError:
            # Item doesn't have a getLogHeader function (ex. it's a float)
            # Try to find it in nameToIndexMap
            varName = None
            for key, val in self.nameToIndexMap.items():
                if val == i:
                    varName = " " + key
            
            # Otherwise just call it stateVariableN
            if varName == None:
                varName = " StateVariable{}".format(i)

            header += varName
    
    return header

class StatefulRigidBody (rigidBodyState, forceParam, inertiaParam, integrationMethod='Euler', discardedTimeStepCallback=None, simDefinition=None)

Intended to represent rigid bodies that have additional stateful properties outside of their rigd body state (ex. tank levels, actuator positions). State is defined a by a StateList instead of by a RigidBodyState.

Just calls RigidBodyState_3DoF constructor

Expand source code

class StatefulRigidBody(RigidBody):
    '''
        Intended to represent rigid bodies that have additional stateful properties outside of their rigd body state (ex. tank levels, actuator positions).
        State is defined a by a `StateList` instead of by a `MAPLEAF.Motion.RigidBodyState`.
    '''
    def __init__(self, rigidBodyState, forceParam, inertiaParam, integrationMethod="Euler", discardedTimeStepCallback=None, simDefinition=None):
        super().__init__(rigidBodyState, forceParam, inertiaParam, integrationMethod, discardedTimeStepCallback, simDefinition)

        self.state = StateList([ rigidBodyState ])
        self.derivativeFuncs = [ self.getRigidBodyStateDerivative ]

    def addStateVariable(self, name, currentValue, derivativeFunction):
        ''' 
            Pass in the current value of a parameter which needs to be integrated, and a derivative function
            Derivative function should accept the current time and StateList as inputs and return the derivative of the new parameter
        '''
        self.state.addStateVariable(name, currentValue)
        self.derivativeFuncs.append(derivativeFunction)

    def getStateDerivative(self, time, state):
        return StateList([ derivativeFunc(time, state) for derivativeFunc in self.derivativeFuncs ], _nameToIndexMap=state.nameToIndexMap)

    def timeStep(self, deltaT):
        # TODO: Current derivative estimates are first-order, replace with a more accurate iterative method
        self.state.estimatedDerivative = self.lastStateDerivative

        integrationResult = self.integrate(self.state, self.time, self.getStateDerivative, deltaT)

        # This is where the simulation time and state are kept track of
        self.time += integrationResult.dt
        self.state = integrationResult.newValue

        # Save for next time step
        self.lastStateDerivative = integrationResult.derivativeEstimate

        return integrationResult

Ancestors

• RigidBody
• RigidBody_3DoF

Methods

def addStateVariable(self, name, currentValue, derivativeFunction)

Pass in the current value of a parameter which needs to be integrated, and a derivative function Derivative function should accept the current time and StateList as inputs and return the derivative of the new parameter

Expand source code

def addStateVariable(self, name, currentValue, derivativeFunction):
    ''' 
        Pass in the current value of a parameter which needs to be integrated, and a derivative function
        Derivative function should accept the current time and StateList as inputs and return the derivative of the new parameter
    '''
    self.state.addStateVariable(name, currentValue)
    self.derivativeFuncs.append(derivativeFunction)

def getStateDerivative(self, time, state)

Expand source code

def getStateDerivative(self, time, state):
    return StateList([ derivativeFunc(time, state) for derivativeFunc in self.derivativeFuncs ], _nameToIndexMap=state.nameToIndexMap)

def timeStep(self, deltaT)

Expand source code

def timeStep(self, deltaT):
    # TODO: Current derivative estimates are first-order, replace with a more accurate iterative method
    self.state.estimatedDerivative = self.lastStateDerivative

    integrationResult = self.integrate(self.state, self.time, self.getStateDerivative, deltaT)

    # This is where the simulation time and state are kept track of
    self.time += integrationResult.dt
    self.state = integrationResult.newValue

    # Save for next time step
    self.lastStateDerivative = integrationResult.derivativeEstimate

    return integrationResult

class Vector (...)

Can be initialized by passing in three numeric components or a single string (ex: "(0, 1, 2)" ) containing numeric values separated by ', ' or ' ', or ';', or '; ' and enclosed by () or {} or [] or no brackets. To initialize from an iterable, unpack the iterable into the constructor with an asterisk: a = np.array([1,2,3]) b = Vector(*a)

Subclasses

• AngularVelocity

Instance variables

var X

Return an attribute of instance, which is of type owner.

var Y

Return an attribute of instance, which is of type owner.

var Z

Return an attribute of instance, which is of type owner.

Methods

def angle(...)

Returns the angle between two vectors, always <= 180 degrees

def crossProduct(...)

def length(...)

def normalize(...)

Returns a unit vector pointing in the same direction as the current Vector