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)
class TimeQuaternion: def setup(self): self.q1 = Quaternion(1, 2, 3, 4) self.q2 = Quaternion(2, 3, 4, 5) self.v1 = Vector(1, 2, 3) def time_multiplyQuaternions(self): q3 = self.q1 * self.q2 def time_rotateVector(self): v2 = self.q1.rotate(self.v1)
def getFinSliceArgs(self, vel, orientation, angVel, fin1, finSlicePosition=0): rocketState = RigidBodyState(Vector(0,0,0), vel, orientation, angVel) CG = self.rocket2.getCG(0, rocketState) rocketVelocity = AeroParameters.getLocalFrameAirVel(rocketState, self.currentConditions) # Add fin velocities due to motion of the rocket velocityDueToRocketPitchYaw = rocketState.angularVelocity.crossProduct(fin1.position - CG)*(-1) #The negative puts it in the wind frame #We need to find the angle between the rocket velocity vector and the plane described by the fin deflection angle #and the shaft normal finNormal = fin1.spanwiseDirection.crossProduct(Vector(0, 0, 1)) finDeflectionAngle = fin1.finAngle # Adjusted by parent finset during each timestep if(finDeflectionAngle != 0): rotation = Quaternion(axisOfRotation = fin1.spanwiseDirection, angle=math.radians(finDeflectionAngle)) finNormal = rotation.rotate(finNormal) finUnitSpanTangentialVelocity = rocketState.angularVelocity.crossProduct(fin1.spanwiseDirection)*(-1) finVelWithoutRoll = rocketVelocity + velocityDueToRocketPitchYaw return finSlicePosition, finVelWithoutRoll, finUnitSpanTangentialVelocity, finNormal, fin1.spanwiseDirection, fin1.stallAngle
def _createReferenceVectors(nCanards, maxAbsCoord, rocketLengthFactor=0.25, finLengthFactor=0.05): ''' Creates a longitudinal vector and an array of nCanards perpendicular vectors. These represent the rocket in the local frame and are rotated according to it's rigid body state at each time step. The size of the longitudinal and perpendicular lines are controlled by the rocketLength and finLength factor arguments and the maxAbsCoord. ''' # Create vector reoresenting longitudinal axis in local frame refAxis = Vector(0, 0, maxAbsCoord * rocketLengthFactor) # Create vectors going perpedicularly out, in the direction of each fin/canard in local frame radiansPerFin = radians(360 / nCanards) finToFinRotation = Quaternion(axisOfRotation=Vector(0, 0, 1), angle=radiansPerFin) perpVectors = [Vector(maxAbsCoord * finLengthFactor, 0, 0)] for i in range(nCanards - 1): newVec = finToFinRotation.rotate(perpVectors[-1]) perpVectors.append(newVec) return refAxis, perpVectors
class TestQuaternion(unittest.TestCase): def setUp(self): self.q1 = Quaternion(components=[1.0, 2.0, 3.0, 4.0]) self.q2 = Quaternion(components=[-1.0, -2.0, -3.0, -4.0]) self.q3 = Quaternion(axisOfRotation=Vector(1.0, 0.0, 0.0), angle=pi / 2) self.q4 = Quaternion(components=[1.0, 0.0, 1.0, 0.0]) self.q5 = Quaternion(components=[1.0, 0.5, 0.5, 0.75]) self.rotQ = Quaternion(axisOfRotation=Vector(2.0, -3.0, 5.0), angle=2) self.rotQ2 = Quaternion(axisOfRotation=Vector(0.0, 0.0, 1.0), angle=pi / 2) self.rotQ3 = Quaternion(axisOfRotation=Vector(0.0, 1.0, 0.0), angle=pi / 2) self.rotQ4 = Quaternion(axisOfRotation=Vector(1.0, 0.0, 0.0), angle=pi / 2) self.rotQ5 = Quaternion(axisOfRotation=Vector(0.0, 0.0, 1.0), angle=pi) self.rotQ6 = Quaternion(axisOfRotation=Vector(0.0, 1.0, 0.0), angle=pi) self.rotQ7 = Quaternion(axisOfRotation=Vector(1.0, 0.0, 0.0), angle=pi) #Test same method as print statement def test_str(self): self.assertEqual(str(self.q1), "<1.0, 2.0, 3.0, 4.0>") def test_scaleRotation(self): scaledQuat = self.q3.scaleRotation(0.5) definedQuat = Quaternion(axisOfRotation=Vector(1, 0, 0), angle=pi / 4) assertQuaternionsAlmostEqual(self, scaledQuat, definedQuat, 14) def test_slerp(self): testQ = self.q1.slerp(self.q3, 0) assertQuaternionsAlmostEqual(self, (self.q1 * testQ).normalize(), self.q1.normalize()) testQ = self.q1.slerp(self.q3, 1) testQ = (self.q1 * testQ).normalize() assertQuaternionsAlmostEqual(self, testQ, self.q3.normalize()) testQ = self.rotQ2.slerp(self.rotQ5, 0.5) testQ = (self.rotQ2 * testQ).normalize() assertQuaternionsAlmostEqual( self, testQ, Quaternion(axisOfRotation=Vector(0, 0, 1), angle=3 * pi / 4)) #Test using the quaternion to rotate a vector #TODO: verify with matlab or other example def test_rotate(self): assertVectorsAlmostEqual(self, self.rotQ2.rotate(Vector(1, 1, 1)), Vector(-1, 1, 1)) assertVectorsAlmostEqual(self, self.rotQ3.rotate(Vector(1, 1, 1)), Vector(1, 1, -1)) assertVectorsAlmostEqual(self, self.rotQ4.rotate(Vector(1, 1, 1)), Vector(1, -1, 1)) assertVectorsAlmostEqual(self, self.rotQ5.rotate(Vector(1, 1, 1)), Vector(-1, -1, 1)) assertVectorsAlmostEqual(self, self.rotQ6.rotate(Vector(1, 1, 1)), Vector(-1, 1, -1)) assertVectorsAlmostEqual(self, self.rotQ7.rotate(Vector(1, 1, 1)), Vector(1, -1, -1)) def test_constructor(self): assertQuaternionsAlmostEqual( self, self.q1, Quaternion(components=[1.0, 2.0, 3.0, 4.0])) expectedQ3 = Quaternion(components=[sin(pi / 4), sin(pi / 4), 0, 0]) assertQuaternionsAlmostEqual(self, self.q3, expectedQ3) #Test == def test_equal(self): self.assertEqual(self.q1, Quaternion(components=[1, 2, 3, 4])) self.assertNotEqual(self.q1, Quaternion(components=[1, 2, 3, 5])) #Check that can't compare Quaterion to a scalar with self.assertRaises(AttributeError): self.q1 == 33 #Test + def test_add(self): self.assertEqual(self.q1 + self.q2, Quaternion(components=[0, 0, 0, 0])) self.assertEqual(self.q1 + self.q1, Quaternion(components=[2, 4, 6, 8])) #Check that adding a scalar and quaternion is not allowed with self.assertRaises(AttributeError): self.q1 + 33 #Test - def test_subtract(self): self.assertEqual(self.q1 - self.q1, Quaternion(components=[0, 0, 0, 0])) self.assertEqual(self.q1 - self.q2, Quaternion(components=[2, 4, 6, 8])) #Check that adding a scalar and quaternion is not allowed with self.assertRaises(AttributeError): self.q1 - 33 #Test * scalar def test_scalarMult(self): self.assertEqual(self.q1 * 2, Quaternion(components=[2, 4, 6, 8])) #Test * quaternion def test_quaternionMult(self): self.assertEqual(self.q4 * self.q5, Quaternion(components=[0.5, 1.25, 1.5, 0.25])) self.assertEqual(self.q4 * self.q4, Quaternion(components=[0, 0, 2, 0])) # Check that order of rotations is left to right rot1 = Quaternion(axisOfRotation=Vector(0, 0, 1), angle=(pi / 2)) rot2 = Quaternion(axisOfRotation=Vector(1, 0, 0), angle=(pi / 2)) totalRotation = rot1 * rot2 initZAxis = Vector(0, 0, 1) finalZAxis = totalRotation.rotate(initZAxis) initXAxis = Vector(1, 0, 0) finalXAxis = totalRotation.rotate(initXAxis) assertVectorsAlmostEqual(self, finalXAxis, Vector(0, 1, 0)) assertVectorsAlmostEqual(self, finalZAxis, Vector(1, 0, 0)) def test_scalarDivision(self): self.assertEqual(self.q1.__truediv__(2), Quaternion(components=[0.5, 1, 1.5, 2])) def test_quaternionDivision(self): testQ = self.q4.__truediv__(self.q5) expectedQ = Quaternion(components=[0.7273, 0.1212, 0.2424, -0.6061]) assertQuaternionsAlmostEqual(self, testQ, expectedQ, 3) def test_norm(self): self.assertEqual(self.q1.norm(), sqrt(30)) def test_normalize(self): testQ = self.q4.normalize() expectedQ = Quaternion(components=[0.7071, 0.0, 0.7071, 0.0]) assertQuaternionsAlmostEqual(self, testQ, expectedQ, 3) def test_conjugate(self): assertQuaternionsAlmostEqual(self, self.q4.conjugate(), Quaternion(components=[1, 0, -1, 0])) def test_inverse(self): assertQuaternionsAlmostEqual( self, self.q4.inverse(), Quaternion(components=[0.5, 0.0, -0.5, 0.0])) def test_rotationAxis(self): axisResult = self.rotQ.rotationAxis() axis = Vector(2, -3, 5).normalize() self.assertAlmostEqual(axisResult.X, axis.X) self.assertAlmostEqual(axisResult.Y, axis.Y) self.assertAlmostEqual(axisResult.Z, axis.Z) def test_rotationAngle(self): self.assertAlmostEqual(self.rotQ.rotationAngle(), 2) def test_plot(self): self.q1.plotRotation(showPlot=False)
def _barrowmanAeroFunc(self, rocketState, time, environment, precomputedData, CG=Vector(0, 0, 0), finDeflectionAngle=None): ''' Precomputed Data is a named tuple (PreComputedFinAeroData) which contains data/results from the parts of the fin aerodynamics calculation that are common to all fins in a FinSet (drag calculations mostly). These calculations are performed at the FinSet level. Only the parts of the Fin Aero Computation that change from fin to fin (normal force mostly, since different fins can have different angles of attack) are computed here ''' Mach = AeroParameters.getMachNumber(rocketState, environment) dynamicPressure = AeroParameters.getDynamicPressure( rocketState, environment) if finDeflectionAngle == None: finDeflectionAngle = self.finAngle # Adjusted by parent finset during each timestep, when the FinSet is controlled # Unpack precomputedData airVelRelativeToFin, CPXPos, totalDragCoefficient = precomputedData #### Compute normal force----------------------------------------------------------------------------------------------------------------- # Find fin normal vector after fin deflection finDeflectionRotation = Quaternion( axisOfRotation=self.spanwiseDirection, angle=radians(finDeflectionAngle)) finNormal = finDeflectionRotation.rotate(self.undeflectedFinNormal) # Get the tangential velocity component vector, per unit radial distance from the rocket centerline rollAngVel = AngularVelocity(0, 0, rocketState.angularVelocity.Z) unitSpanTangentialAirVelocity = rollAngVel.crossProduct( self.spanwiseDirection) * (-1) def subsonicNormalForce(Mach): # Subsonic linear method tempBeta = AeroParameters.getBeta(Mach) CnAlpha = getFinCnAlpha_Subsonic_Barrowman(self.span, self.planformArea, tempBeta, self.midChordSweep) return getSubsonicFinNormalForce(airVelRelativeToFin, unitSpanTangentialAirVelocity, finNormal, self.spanwiseDirection, self.CPSpanWisePosition.length(), CnAlpha, self) def supersonicNormalForce(Mach): # Supersonic Busemann method gamma = AeroFunctions.getGamma() tempBeta = AeroParameters.getBeta(Mach) K1, K2, K3, Kstar = getBusemannCoefficients(Mach, tempBeta, gamma) # Mach Cone coords machAngle = asin(1 / Mach) machCone_negZPerRadius = 1 / tan(machAngle) machConeEdgeZPos = [] outerRadius = self.spanSliceRadii[-1] for i in range(len(self.spanSliceRadii)): machConeAtCurrentRadius = ( outerRadius - self.spanSliceRadii[i] ) * machCone_negZPerRadius + self.tipPosition machConeEdgeZPos.append(machConeAtCurrentRadius) return getSupersonicFinNormalForce( airVelRelativeToFin, unitSpanTangentialAirVelocity, finNormal, machConeEdgeZPos, self.spanwiseDirection, self.CPSpanWisePosition.length(), K1, K2, K3, Kstar, self) if Mach <= 0.8: normalForceMagnitude, finMoment = subsonicNormalForce(Mach) elif Mach < 1.4: # Interpolate in transonic region # TODO: Do this with less function evaluations? Perhaps precompute AOA and Mach combinations and simply interpolate? Lazy precompute? Cython? x1, x2 = 0.8, 1.4 # Start, end pts of interpolated region dx = 0.001 # Find normal force and derivative at start of interpolation interval f_x1, m_x1 = subsonicNormalForce(x1) f_x12, m_x12 = subsonicNormalForce(x1 + dx) # Find normal force and derivative at end of interpolation interval f_x2, m_x2 = supersonicNormalForce(x2) f_x22, m_x22 = supersonicNormalForce(x2 + dx) normalForceMagnitude = Interpolation.cubicInterp( Mach, x1, x2, f_x1, f_x2, f_x12, f_x22, dx) finMoment = Interpolation.cubicInterp(Mach, x1, x2, m_x1, m_x2, m_x12, m_x22, dx) else: normalForceMagnitude, finMoment = supersonicNormalForce(Mach) # Complete redimensionalization of normal force coefficients by multiplying by dynamic pressure # Direct the normal force along the fin's normal direction normalForce = normalForceMagnitude * dynamicPressure * finNormal finMoment *= dynamicPressure #### Get axial force----------------------------------------------------------------------------------------------------------------------- avgAOA = getFinSliceAngleOfAttack( self.spanSliceRadii[round(len(self.spanSliceAreas) / 2)], airVelRelativeToFin, unitSpanTangentialAirVelocity, finNormal, self.spanwiseDirection, self.stallAngle) # Approximate average fin AOA totalAxialForceCoefficient = AeroFunctions.getDragToAxialForceFactor( avgAOA) * totalDragCoefficient axialForceMagnitude = totalAxialForceCoefficient * self.rocket.Aref * dynamicPressure axialForceDirection = self.spanwiseDirection.crossProduct(finNormal) axialForce = axialForceDirection * axialForceMagnitude #### Get CP Location ---------------------------------------------------------------------------------------------------------------------- CPChordWisePosition = self.position - Vector( 0, 0, CPXPos ) # Ignoring the change in CP position due to fin deflection for now globalCP = self.CPSpanWisePosition + CPChordWisePosition #### Assemble total force moment system objects-------------------------------------------------------------------------------------------- totalForce = normalForce + axialForce return ForceMomentSystem(totalForce, globalCP, moment=Vector(0, 0, finMoment)), globalCP
def getDirection(self, time): ''' Launch Rail moves with the earth's surface ''' rotationSoFar = Quaternion(axisOfRotation=Vector(0, 0, 1), angle=(self.earthAngVel * time)) return rotationSoFar.rotate(self.initialDirection)
def complexRotationTest(self, integrationMethod): ''' Test where the rigid body is first accelerated and decelerated about the y-axis, completing a 90 degree rotaion, subsequently does the same about the z-axis ''' initXAxis = Vector(1, 0, 0) initYAxis = Vector(0, 1, 0) initZAxis = Vector(0, 0, 1) initOrientation = Quaternion(axisOfRotation=Vector(1, 0, 0), angle=0) initAngularMomentum = AngularVelocity(rotationVector=self.zeroVec) complexRotatingState = RigidBodyState(self.zeroVec, self.zeroVec, initOrientation, initAngularMomentum) def step1MomentFunc(*allArgs): return ForceMomentSystem( self.zeroVec, moment=Vector(0, 0.5, 0) ) #Acceleration and deceleration steps to give 90 degree rotations in each axis def step2MomentFunc(*allArgs): return ForceMomentSystem(self.zeroVec, moment=Vector( 0, -0.5, 0)) #using a time step of pi def step3MomentFunc(*allArgs): return ForceMomentSystem( self.zeroVec, moment=Vector(0, 0, 0.5) ) #There is one acceleration and one deceleration for each rotation to make it static def step4MomentFunc(*allArgs): return ForceMomentSystem(self.zeroVec, moment=Vector(0, 0, -0.5)) def constInertiaFunc(*allArgs): return self.constInertia # Create rigid body and apply moments for one time step each self.simDefinition.setValue( "SimControl.TimeStepAdaptation.minTimeStep", str(math.sqrt(math.pi))) complexRotatingBody = RigidBody(complexRotatingState, step1MomentFunc, constInertiaFunc, integrationMethod=integrationMethod, simDefinition=self.simDefinition) complexRotatingBody.integrate.firstSameAsLast = False # First same as last causes problems here - perhaps with the constantly changing integration function complexRotatingBody.timeStep(math.sqrt( math.pi)) #Apply each moment and do the timestep complexRotatingBody.forceFunc = step2MomentFunc complexRotatingBody.timeStep(math.sqrt(math.pi)) complexRotatingBody.forceFunc = step3MomentFunc complexRotatingBody.timeStep(math.sqrt(math.pi)) complexRotatingBody.forceFunc = step4MomentFunc complexRotatingBody.timeStep(math.sqrt(math.pi)) newXVector = complexRotatingBody.state.orientation.rotate( initXAxis ) #Calculate the new vectors from the integration in the rigid body newYVector = complexRotatingBody.state.orientation.rotate(initYAxis) newZVector = complexRotatingBody.state.orientation.rotate(initZAxis) correctRotation1 = Quaternion( axisOfRotation=initYAxis, angle=math.pi / 2) #Calculate the new vectors by hand (90 degree rotations) newCorrectXAxis1 = correctRotation1.rotate(initXAxis) # (0, 0, -1) newCorrectYAxis1 = correctRotation1.rotate(initYAxis) # (0, 1, 0) newCorrectZAxis1 = correctRotation1.rotate(initZAxis) # (1, 0, 0) correctRotation2Axis = newCorrectZAxis1 #(1, 0, 0) correctRotation2 = Quaternion(axisOfRotation=correctRotation2Axis, angle=math.pi / 2) newCorrectXAxis2 = correctRotation2.rotate( newCorrectXAxis1) # (0, 1, 0) newCorrectYAxis2 = correctRotation2.rotate( newCorrectYAxis1) # (0, 0, 1) newCorrectZAxis2 = correctRotation2.rotate( newCorrectZAxis1) # (1, 0, 0) assertVectorsAlmostEqual(self, newXVector, newCorrectXAxis2) assertVectorsAlmostEqual(self, newYVector, newCorrectYAxis2) assertVectorsAlmostEqual(self, newZVector, newCorrectZAxis2)