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)