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()