def getTurbVelocity(self, altitude, meanWindVelocity, time):
''' # From NASA HDBK-1001, pg. 2-84, equation 2.69 '''
# Determine whether the current altitude is in the blending region or plateau region
if altitude > self.GustStartAltitude:
if altitude < self.GustZoneBoundaries[1]:
# In the lower blending region
gustVel = self.GustMagnitude / 2 * (
1 - cos(pi * (altitude - self.GustStartAltitude) /
self.GustSineBlendDistance))
elif altitude < self.GustZoneBoundaries[2]:
# In the plateau (gust) region
gustVel = self.GustMagnitude
elif altitude < self.GustZoneBoundaries[3]:
# In the upper blending region
gustThickness = self.GustZoneBoundaries[
-1] - self.GustZoneBoundaries[0]
gustVel = self.GustMagnitude / 2 * (1 - cos(
pi * (altitude - self.GustStartAltitude - gustThickness) /
self.GustSineBlendDistance))
else:
return Vector(0, 0, 0)
return self.GustDirection * gustVel
else:
return Vector(0, 0, 0)
def test_compositeObjectOfCompositeObjects(self):
ballMOI = Vector(0.16, 0.16, 0.16)
ballCentroid1 = Vector(-0.4, 0, 0)
ballCentroid2 = Vector(0.4, 0, 0)
ballCentroid3 = Vector(1.2, 0, 0)
ballCentroid4 = Vector(2.0, 0, 0)
ball1Inertia = Inertia(ballMOI, ballCentroid1, 10)
ball2Inertia = Inertia(ballMOI, ballCentroid2, 20)
ball3Inertia = Inertia(ballMOI, ballCentroid3, 30)
ball4Inertia = Inertia(ballMOI, ballCentroid4, 40)
Ball1 = FixedMassForCompositeObjectTest(ball1Inertia)
Ball2 = FixedMassForCompositeObjectTest(ball2Inertia)
Ball3 = FixedMassForCompositeObjectTest(ball3Inertia)
Ball4 = FixedMassForCompositeObjectTest(ball4Inertia)
innerCompObject1 = CompositeObject([Ball1, Ball2])
innerCompObject2 = CompositeObject([Ball3, Ball4])
completeCompositeObject = CompositeObject([Ball1, Ball2, Ball3, Ball4])
outerCompObject = CompositeObject([innerCompObject1, innerCompObject2])
# Check whether we get the same results from outer and complete composite objects
innerMOI = outerCompObject.getInertia(0, None).MOI
completeMOI = completeCompositeObject.getInertia(0, None).MOI
assertIterablesAlmostEqual(self, innerMOI, completeMOI)
def _ensureBaseDragIsAccountedFor(self):
''' If no BoatTail exists at the bottom of the rocket, adds a zero-length boat tail. This is necessary b/c Boat Tail aero-functions are the ones that account for base drag '''
boatTailComponentAtBottomOfRocket = False
bottomStage = self.stages[-1]
for comp in reversed(bottomStage.components):
if isinstance(comp, BodyComponent):
if isinstance(comp, BoatTail):
boatTailComponentAtBottomOfRocket = True
break
if not boatTailComponentAtBottomOfRocket and self.addZeroLengthBoatTailsToAccountForBaseDrag:
if not self.silent:
print("Adding zero-length BoatTail to the bottom of current bottom stage ({}) to account for base drag".format(bottomStage.name))
# Create a zero-length, zero-mass boat tail to account for base drag
zeroInertia = Inertia(Vector(0,0,0), Vector(0,0,0), 0)
diameter = self.maxDiameter # TODO: Get the actual bottom-body-tube diameter from a future Stage.getRadius function
length = 0
position = bottomStage.getBottomInterfaceLocation()
boatTail = BoatTail(
diameter,
diameter,
length,
position,
zeroInertia,
self,
bottomStage,
"Auto-AddedZeroLengthBoatTail",
self.surfaceRoughness
)
initializeForceLogging(boatTail, "FakeRocketName.Auto-AddedZeroLengthBoatTail", self)
bottomStage.components.append(boatTail)
def test_TabulatedAeroForce(self):
# Init rocket that uses tabulated aero data
simDef = SimDefinition("MAPLEAF/Examples/Simulations/NASATwoStageOrbitalRocket.mapleaf", silent=True)
rocketDictReader = SubDictReader("Rocket", simDef)
rocket = Rocket(rocketDictReader, silent=True)
comp1, comp2, comp3 = rocket.stages[0].getComponentsOfType(TabulatedAeroForce)
if comp1.Lref == 3.0:
tabMoment = comp1
tabFOrce = comp2
else:
tabMoment = comp2
tabForce = comp1
zeroAOAState = RigidBodyState(Vector(0,0,0), Vector(0,0,100), Quaternion(1,0,0,0), Vector(0,0,0))
# CD, CL, CMx, CMy, CMz
expectedAero = [ 0.21, 0, 0, 0, 0 ]
calculatedAero = tabForce._getAeroCoefficients(zeroAOAState, self.currentConditions)
assertIterablesAlmostEqual(self, expectedAero, calculatedAero)
expectedAero = [ 0, 0, 0, 0, 0 ]
calculatedAero = tabMoment._getAeroCoefficients(zeroAOAState, self.currentConditions)
assertIterablesAlmostEqual(self, expectedAero, calculatedAero)
def test_combineInertiasAboutPoint_TShapedBlock(self):
# http://adaptivemap.ma.psu.edu/websites/A2_moment_intergrals/parallel_axis_theorem/pdf/ParallelAxis_WorkedProblem2.pdf
# T-shaped block
# Top block (1) dimensions are (4,2,0)
# Bottom block (2) dimensions are (2,4,0)
Ixx1 = 1 / 12 * 2 * 4**3
Ixx2 = 1 / 12 * 4 * 2**3
Iyy1 = Ixx2
Iyy2 = Ixx1
MOI1 = Vector(Ixx1, Iyy1, 0)
centroid1 = Vector(0, 3, 0)
mass1 = 8
inertia1 = Inertia(MOI1, centroid1, mass1, centroid1)
MOI2 = Vector(Ixx2, Iyy2, 0)
centroid2 = Vector(0, 0, 0)
mass2 = 8
inertia2 = Inertia(MOI2, centroid2, mass2, centroid2)
# Add them up at the overall centroid
overallCentroid = Vector(0, 1.5, 0)
combinedInertia = inertia1.combineInertiasAboutPoint([inertia2],
overallCentroid)
self.assertAlmostEqual(combinedInertia.MOI.X, 49.333333333333)
self.assertAlmostEqual(combinedInertia.mass, 16)
self.assertAlmostEqual(combinedInertia.CG, overallCentroid)
self.assertAlmostEqual(combinedInertia.MOICentroidLocation,
overallCentroid)
def test_InertiaAddition(self):
mCG1 = Inertia(Vector(0, 0, 0), Vector(0, 0, 2), 10)
mCG2 = Inertia(Vector(0, 0, 0), Vector(0, 0, 0), 20)
m3 = mCG1.combineInertiasAboutPoint([mCG2], Vector(0, 0, 0))
self.assertEqual(m3.mass, 30)
self.assertAlmostEqual(m3.CG.Z, 0.6666666666666)
class AeroForce(RocketComponent):
''' A zero-Inertia component with constant aerodynamic coefficients '''
# Object is just a force, inertia is zero
inertia = Inertia(Vector(0, 0, 0), Vector(0, 0, 0), 0)
def __init__(self, componentDictReader, rocket, stage):
self.componentDictReader = componentDictReader
self.rocket = rocket
self.stage = stage
self.name = componentDictReader.getDictName()
self.position = componentDictReader.getVector("position")
self.Aref = componentDictReader.getFloat("Aref")
self.Lref = componentDictReader.getFloat("Lref")
Cd = componentDictReader.getFloat("Cd")
Cl = componentDictReader.getFloat("Cl")
momentCoeffs = componentDictReader.getVector("momentCoeffs")
self.aeroCoeffs = [Cd, Cl, *momentCoeffs]
def getInertia(self, time, state):
return self.inertia
def getAppliedForce(self, state, time, environment, rocketCG):
return AeroFunctions.forceFromCoefficients(state, environment,
*self.aeroCoeffs,
self.position, self.Aref,
self.Lref)
def __init__(self, componentDictReader, rocket, stage):
self.rocket = rocket
self.stage = stage
self.componentDictReader = componentDictReader
self.name = componentDictReader.getDictName()
mass = componentDictReader.getFloat("mass")
# Position in simulation definition is relative to stage position
self.position = componentDictReader.getVector(
"position"
) + stage.position # Store position relative to nosecone here
# CG in simulation definition is relative to component position
cg = componentDictReader.getVector(
"cg"
) + self.position # Store cg location relative to nosecone here
try:
MOI = componentDictReader.getVector("MOI")
except:
MOI = Vector(mass * 0.01, mass * 0.01,
mass * 0.01) # Avoid having zero moments of inertia
self.inertia = Inertia(MOI, cg, mass)
self.zeroForce = ForceMomentSystem(Vector(0, 0, 0))
def uniformXAccelerationTest(self, integrationMethod):
constantXForce = ForceMomentSystem(Vector(1, 0, 0))
movingXState = RigidBodyState(self.zeroVec, self.zeroVec,
self.zeroQuat, self.zeroAngVel)
def constInertiaFunc(*allArgs):
return self.constInertia
def constXForceFunc(*allArgs):
return constantXForce
movingXBody = RigidBody(movingXState,
constXForceFunc,
constInertiaFunc,
integrationMethod=integrationMethod,
simDefinition=self.simDefinition)
movingXBody.timeStep(1)
newPos = movingXBody.state.position
newVel = movingXBody.state.velocity
assertVectorsAlmostEqual(self, newVel, Vector(1, 0, 0))
if integrationMethod == "Euler":
assertVectorsAlmostEqual(self, newPos, Vector(0, 0, 0))
else:
assertVectorsAlmostEqual(self, newPos, Vector(0.5, 0, 0))
def forceFromCoefficients(rocketState, environment, Cd, Cl, CMx, CMy, CMz,
CPLocation, refArea, refLength):
''' Initialize ForceMomentSystem from all aerodynamic coefficients '''
q = AeroParameters.getDynamicPressure(rocketState, environment)
if q == 0.0:
# No force without dynamic pressure
return ForceMomentSystem(Vector(0, 0, 0))
nonDimConstant = q * refArea
#### Drag ####
localFrameAirVel = AeroParameters.getLocalFrameAirVel(
rocketState, environment)
dragDirection = localFrameAirVel.normalize()
dragForce = dragDirection * Cd
#### Lift ####
# Find the lift force direction
# Lift force will be in the same plane as the normalForceDirection & rocketFrameAirVel vectors
# But, the lift force will be perpendicular to rocketFraeAirVel, whereas the normalForceDirection might form an acute angle with it
# Rotate the normal force direction vector until it's perpendicular with rocketFrameAirVel
normalForceDir = AeroParameters.getNormalAeroForceDirection(
rocketState, environment)
rotAxis = localFrameAirVel.crossProduct(normalForceDir)
angle = math.pi / 2 - localFrameAirVel.angle(normalForceDir)
rotatorQuat = Quaternion(rotAxis, angle)
liftDirection = rotatorQuat.rotate(normalForceDir)
liftForce = liftDirection * Cl
# Combine and redimensionalize
totalForce = (dragForce + liftForce) * nonDimConstant
#### Moments ####
moments = Vector(CMx, CMy, CMz) * nonDimConstant * refLength
return ForceMomentSystem(totalForce, CPLocation, moments)
def test_UniformXMotionAndScalarIntegration(self):
movingXState = RigidBodyState(self.zeroVec, Vector(1, 0, 0),
self.zeroQuat, self.zeroAngVel)
constInertia = Inertia(self.oneVec, self.zeroVec, 1)
def constInertiaFunc(*allArgs):
return constInertia
def zeroForceFunc(*allArgs):
return self.zeroForce
movingXBody = StatefulRigidBody(movingXState,
zeroForceFunc,
constInertiaFunc,
integrationMethod="RK4",
simDefinition=self.simDefinition)
movingXBody.addStateVariable("scalarVar", 3, sampleDerivative)
movingXBody.timeStep(0.2)
# Check that the rigid body state has been integrated correctly
newPos = movingXBody.state[0].position
assertVectorsAlmostEqual(self, newPos, Vector(0.2, 0, 0))
# Check that the scalar function has been integrated correctly
finalVal = movingXBody.state[1]
self.assertAlmostEqual(finalVal, 2.0112)
def test_dualUniformXMotion(self):
movingXState = RigidBodyState(self.zeroVec, Vector(1, 0, 0),
self.zeroQuat, self.zeroAngVel)
constInertia = Inertia(self.oneVec, self.zeroVec, 1)
def constInertiaFunc(*allArgs):
return constInertia
def zeroForceFunc(*allArgs):
return self.zeroForce
movingXBody = StatefulRigidBody(movingXState,
zeroForceFunc,
constInertiaFunc,
simDefinition=self.simDefinition)
# Add the same rigid body state and state derivative function as a second state variable to be integrated
movingXBody.addStateVariable("secondRigidBodyState", movingXState,
movingXBody.getRigidBodyStateDerivative)
movingXBody.timeStep(1)
# Check that both rigid body states have been integrated correctly
newPos = movingXBody.state[0].position
newPos2 = movingXBody.state[1].position
assertVectorsAlmostEqual(self, newPos, Vector(1, 0, 0))
assertVectorsAlmostEqual(self, newPos2, Vector(1, 0, 0))
def uniformlyAcceleratingRotationTest(self, integrationMethod):
constantZMoment = ForceMomentSystem(self.zeroVec,
moment=Vector(0, 0, 1))
restingState = RigidBodyState(self.zeroVec, self.zeroVec,
self.zeroQuat, self.zeroAngVel)
def constZMomentFunc(*allArgs):
return constantZMoment
def constInertiaFunc(*allArgs):
return self.constInertia
acceleratingZRotBody = RigidBody(restingState,
constZMomentFunc,
constInertiaFunc,
integrationMethod=integrationMethod,
simDefinition=self.simDefinition)
acceleratingZRotBody.timeStep(1)
newOrientation = acceleratingZRotBody.state.orientation
newAngVel = acceleratingZRotBody.state.angularVelocity
#print("{}: {}".format(integrationMethod, newOrientation))
assertAngVelAlmostEqual(
self, newAngVel,
AngularVelocity(axisOfRotation=Vector(0, 0, 1), angularVel=1))
if integrationMethod == "Euler":
assertQuaternionsAlmostEqual(
self, newOrientation,
Quaternion(axisOfRotation=Vector(0, 0, 1), angle=0))
else:
assertQuaternionsAlmostEqual(
self, newOrientation,
Quaternion(axisOfRotation=Vector(0, 0, 1), angle=0.5))
def assertForceMomentSystemsAlmostEqual(TestCaseObject, fms1, fms2, n=7):
# Convert both systems to be defined about the origin
fms1 = fms1.getAt(Vector(0, 0, 0))
fms2 = fms2.getAt(Vector(0, 0, 0))
# Then check that forces and moments are almost equal
assertVectorsAlmostEqual(TestCaseObject, fms1.force, fms2.force, n)
assertVectorsAlmostEqual(TestCaseObject, fms1.moment, fms2.moment, n)
def getNormalAeroForceDirection(state, environment):
localFrameAirVel = getLocalFrameAirVel(state, environment)
if localFrameAirVel[0] == 0.0 and localFrameAirVel[1] == 0.0:
# Return random vector if AOA == 0 - force will be zero anyhow
return Vector(1, 0, 0)
else:
return Vector(localFrameAirVel[0], localFrameAirVel[1], 0).normalize()
def test_DetectTimeReached(self):
self.eventDetector.subscribeToEvent(EventTypes.TimeReached,
self.exampleCallbackFunction,
eventTriggerValue=30.01)
self.rocket.rigidBody.state.position = Vector(0, 0, 9999.999)
self.rocket.rigidBody.state.velocity = Vector(0, 0, 1)
self._checkEventTriggering()
def getInertia(self, time, rocketState):
mass = 5 + rocketState.tankLevel * 4.56 # Fixed Mass + fluid mass
MOI = Vector(mass, mass, mass * 0.05) # Related to current mass
CGz = -3 + rocketState.tankLevel # Moves depending on current tank level
CG = Vector(0, 0, CGz)
return Inertia(MOI, CG, mass)
def test_combineForceMomentSystems(self):
combinedForce = self.appliedForce1 + self.appliedForce2
forceAtCG = combinedForce.getAt(Vector(0, 0, 0))
self.assertEqual(forceAtCG, self.correctForce1)
combinedForce2 = self.appliedForce1 + self.appliedForce3
forceAtCG2 = combinedForce2.getAt(Vector(0, 0, 0))
self.assertEqual(forceAtCG2, self.correctForce2)
def test_CPZ(self):
force = Vector(0, 1, 0)
location = Vector(0, 0, 0)
moment = Vector(1, 0, 0)
testForce = ForceMomentSystem(force, location, moment)
expectedCPZ = -1
calculatedCPZ = AeroFunctions._getCPZ(testForce)
self.assertAlmostEqual(expectedCPZ, calculatedCPZ)
def setUp(self):
pos1 = Vector(0, 1, 2)
pos2 = Vector(1, 2, 3)
Vel1 = Vector(-1, 0, 1)
Vel2 = Vector(2, 3, 4)
self.iRBS1 = RigidBodyState_3DoF(pos1, Vel1)
self.iRBS2 = RigidBodyState_3DoF(pos2, Vel2)
def test_fixedForceInit(self):
simDef = SimDefinition("MAPLEAF/Examples/Simulations/FixedForce.mapleaf", silent=True)
rocketDictReader = SubDictReader("Rocket", simDef)
rocket = Rocket(rocketDictReader, silent=True)
fixedForce = rocket.stages[0].getComponentsOfType(FixedForce)[0]
expectedFMS = ForceMomentSystem(Vector(0,0,0), Vector(0,0,0), Vector(0,1,0))
assertForceMomentSystemsAlmostEqual(self, fixedForce.force, expectedFMS)
def test_DetectAscendingThroughAltitude(self):
# Subscribing with string
self.eventDetector.subscribeToEvent("ascendingThroughAltitude",
self.exampleCallbackFunction,
eventTriggerValue=10000)
self.rocket.rigidBody.state.position = Vector(0, 0, 9999.999)
self.rocket.rigidBody.state.velocity = Vector(0, 0, 1)
self._checkEventTriggering()
def test_MotorGetOxInertia(self):
motor = self.motorRocket.stages[0].getComponentsOfType(
TabulatedMotor)[0]
assertInertiasAlmostEqual(
self, motor._getOxInertia(0),
Inertia(Vector(1, 1, 0.1), Vector(0, 0, 2), 9.99))
assertInertiasAlmostEqual(
self, motor._getOxInertia(5),
Inertia(Vector(0, 0, 0), Vector(0, 0, 2.5), 0.0))
def test_MotorGetFuelInertia(self):
motor = self.motorRocket.stages[0].getComponentsOfType(
TabulatedMotor)[0]
assertInertiasAlmostEqual(
self, motor._getFuelInertia(0),
Inertia(Vector(0.15, 0.15, 0.02), Vector(0, 0, 3),
1.4985000000000002))
assertInertiasAlmostEqual(self, motor._getFuelInertia(5),
Inertia(Vector(0, 0, 0), Vector(0, 0, 3), 0))
def test_AddRigidBodyState(self):
iRBS3 = self.iRBS1 + self.iRBS2
self.assertEqual(iRBS3.position, Vector(1, 0, 0))
self.assertEqual(iRBS3.velocity, Vector(2, 0, 0))
self.assertEqual(
iRBS3.orientation,
Quaternion(axisOfRotation=Vector(1, 0, 0), angle=pi / 2))
self.nearlyEqualAngVel(
iRBS3.angularVelocity,
AngularVelocity(axisOfRotation=Vector(1, 0, 0), angularVel=pi))
def test_sendIMUData(self):
simTime = time.time() + 1000
state1 = RigidBodyState(
Vector(0, 0, 250), Vector(0, 0, 250),
Quaternion(axisOfRotation=Vector(0, 0, 1), angle=3.141592654),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.interface.sendIMUData(state1, simTime)
def setUp(self):
zeroVec = Vector(0, 0, 0)
zeroQuat = Quaternion(axisOfRotation=Vector(0, 0, 1), angle=0)
zeroAngVel = AngularVelocity(rotationVector=zeroVec)
self.initState = RigidBodyState(zeroVec, zeroVec, zeroQuat, zeroAngVel)
startPos = zeroVec
direction = Vector(1, 0, 1)
length = 5
self.rail = LaunchRail(startPos, direction, length)
def test_zeroSag(self):
unadjustedForce = ForceMomentSystem(Vector(10, 9, -10),
moment=Vector(10, 11, 12))
adjustedForce = self.rail.applyLaunchTowerForce(
self.initState, 0, unadjustedForce)
# Check that force is zero (don't let object sag/fall)
assertVectorsAlmostEqual(self, adjustedForce.force, Vector(0, 0, 0))
# Check moments are zero
assertVectorsAlmostEqual(self, adjustedForce.moment, Vector(0, 0, 0))
def test_getFixedForceInertia(self):
simDef = SimDefinition("MAPLEAF/Examples/Simulations/FixedForce.mapleaf", silent=True)
rocketDictReader = SubDictReader("Rocket", simDef)
rocket = Rocket(rocketDictReader, silent=True)
fixedForce = rocket.stages[0].getComponentsOfType(FixedForce)[0]
inertia = fixedForce.getInertia(0, "fakeState")
expectedInertia = Inertia(Vector(0,0,0), Vector(0,0,0), 0)
self.assertEqual(inertia, expectedInertia)
def test_DetectDescendingThroughAltitude(self):
self.eventDetector.subscribeToEvent(
EventTypes.DescendingThroughAltitude,
self.exampleCallbackFunction,
eventTriggerValue=100)
self.rocket.rigidBody.state.position = Vector(0, 0, 100.001)
self.rocket.rigidBody.state.velocity = Vector(0, 0, -1)
self._checkEventTriggering()
