def test_RigidBodyState_add(self):
pos = Vector(0, 0, 0)
vel = Vector(0, 0, 0)
orientation = Quaternion(axisOfRotation=Vector(0, 0, 1), angle=0)
angVel = AngularVelocity(rotationVector=Vector(0, 0, 0))
state1 = RigidBodyState(pos, vel, orientation, angVel)
pos2 = Vector(0, 0, 0.1)
vel2 = Vector(0, 0, 0.2)
orientation2 = Quaternion(axisOfRotation=Vector(0, 0, 1), angle=pi / 2)
angVel2 = AngularVelocity(rotationVector=Vector(0, 0, 0.3))
state2 = RigidBodyState(pos2, vel2, orientation2, angVel2)
result = state1 + state2
assertRigidBodyStatesalmostEqual(self, result, state2)
pos3 = Vector(0, 0, 0.1)
vel3 = Vector(0, 0, 0.2)
orientation3 = Quaternion(axisOfRotation=Vector(0, 1, 0), angle=pi / 2)
angVel3 = AngularVelocity(rotationVector=Vector(0, 0, 0.3))
state3 = RigidBodyState(pos3, vel3, orientation3, angVel3)
result = state3 + state2
testVec = result.orientation.rotate(Vector(0, 0, 1))
assertVectorsAlmostEqual(self, testVec, Vector(0, 1, 0))
def test_getNormalAeroForceDirection(self):
zeroAngVel = AngularVelocity(0,0,0)
zeroPos = Vector(0,0,0)
state1 = RigidBodyState(zeroPos, self.dummyVelocity1, self.dummyOrientation1, zeroAngVel)
assertVectorsAlmostEqual(self, AeroParameters.getNormalAeroForceDirection(state1, self.currentConditions), Vector(0, -1, 0))
state2 = RigidBodyState(zeroPos, self.dummyVelocity1, self.dummyOrientation2, zeroAngVel)
assertVectorsAlmostEqual(self, AeroParameters.getNormalAeroForceDirection(state2, self.currentConditions), Vector(0, 1, 0))
state3 = RigidBodyState(zeroPos, self.dummyVelocity1, self.dummyOrientation3, zeroAngVel)
assertVectorsAlmostEqual(self, AeroParameters.getNormalAeroForceDirection(state3, self.currentConditions), Vector(1, 0, 0))
state4 = RigidBodyState(zeroPos, self.dummyVelocity1, self.dummyOrientation4, zeroAngVel)
assertVectorsAlmostEqual(self, AeroParameters.getNormalAeroForceDirection(state4, self.currentConditions), Vector(1, 0, 0))
state5 = RigidBodyState(zeroPos, self.dummyVelocity1, self.dummyOrientation5, zeroAngVel)
assertVectorsAlmostEqual(self, AeroParameters.getNormalAeroForceDirection(state5, self.currentConditions), Vector(0.707, -0.707, 0),3)
assertVectorsAlmostEqual(self, AeroParameters.getNormalAeroForceDirection(self.rocketState8, self.currentConditions), Vector(-0.99999, 0.0022736, 0),3)
# Prep rocket by calling getLocalFrameAirVel first to cache AOA result, then get cached result from getTotalAOA
def testLocalFrameAirVelNormalForceDir(vel, orientation, expectedResult, precision=7):
state = RigidBodyState(Vector(0,0,0), vel, orientation, AngularVelocity(0,0,0))
assertVectorsAlmostEqual(self, AeroParameters.getNormalAeroForceDirection(state, self.currentConditions), expectedResult, precision)
testLocalFrameAirVelNormalForceDir(self.dummyVelocity1, self.dummyOrientation1, Vector(0, -1, 0))
testLocalFrameAirVelNormalForceDir(self.dummyVelocity1, self.dummyOrientation2, Vector(0, 1, 0))
testLocalFrameAirVelNormalForceDir(self.dummyVelocity1, self.dummyOrientation3, Vector(1, 0, 0))
testLocalFrameAirVelNormalForceDir(self.dummyVelocity1, self.dummyOrientation4, Vector(1, 0, 0))
testLocalFrameAirVelNormalForceDir(self.dummyVelocity1, self.dummyOrientation5, Vector(0.707, -0.707, 0), 3)
testLocalFrameAirVelNormalForceDir(self.rocketState8.velocity, self.rocketState8.orientation, Vector(-0.99999, 0.0022736, 0),3)
def test_fullInterface(self):
state = RigidBodyState(
Vector(0, 0, 0), Vector(0, 0, 0),
Quaternion(axisOfRotation=Vector(0, 0, 1), angle=0),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.interface.setupHIL(state)
#self.interface.teensyComs.close()
print("go")
#time.sleep(5)
for i in range(1000):
print("Working")
state = RigidBodyState(
Vector(0, 0, i), Vector(0, 0, i),
Quaternion(axisOfRotation=Vector(0, 0, 1),
angle=i * 0.0003141592654),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.interface.sendIMUData(state, i * 5)
time.sleep(0.001)
self.interface.requestCanardAngles()
print(self.interface.getCanardAngles())
def setUp(self):
pos1 = Vector(0, 0, 0)
pos2 = Vector(1, 0, 0)
Vel1 = Vector(0, 0, 0)
Vel2 = Vector(2, 0, 0)
Orientation1 = Quaternion(axisOfRotation=Vector(1, 0, 0), angle=0)
Orientation2 = Quaternion(axisOfRotation=Vector(1, 0, 0), angle=pi / 2)
AngVel1 = AngularVelocity(axisOfRotation=Vector(1, 0, 0), angularVel=0)
AngVel2 = AngularVelocity(axisOfRotation=Vector(1, 0, 0),
angularVel=pi)
self.iRBS1 = RigidBodyState(pos1, Vel1, Orientation1, AngVel1)
self.iRBS2 = RigidBodyState(pos2, Vel2, Orientation2, AngVel2)
def test_getVelocityByteArray(self):
state1 = RigidBodyState(
Vector(0, 0, 250), Vector(0, 0, 250),
Quaternion(Vector(0, 0, 1), 0),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
state2 = RigidBodyState(
Vector(0, 0, -250), Vector(0, 0, -250),
Quaternion(Vector(0, 0, 0), 180),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.assertEqual(self.interface.getVelocityByteArray(state1),
(0, 0, 0, 0, 0, 0, 0, 0, 195, 122, 0, 0, 0, 0, 0, 1))
self.assertEqual(self.interface.getVelocityByteArray(state2),
(0, 0, 0, 0, 0, 0, 0, 0, 67, 122, 0, 0, 0, 0, 0, 1))
def test_getQuaternionByteArray(self):
state1 = RigidBodyState(
Vector(0, 0, 200), Vector(0, 0, 200),
Quaternion(Vector(0, 0, 1), 0),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
state2 = RigidBodyState(
Vector(0, 0, 200), Vector(0, 0, 200),
Quaternion(Vector(0, 0, 1), 180),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.assertEqual(self.interface.getQuaternionByteArray(state1),
(116, 93, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1))
self.assertEqual(self.interface.getQuaternionByteArray(state2),
(203, 221, 0, 0, 0, 0, 104, 7, 0, 0, 0, 1))
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 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_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_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 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 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_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):
simDef = SimDefinition("MAPLEAF/Examples/Simulations/test3.mapleaf")
rocketDictReader = SubDictReader("Rocket", simDef)
self.rocket = Rocket(rocketDictReader)
self.environment = Environment(silent=True)
self.currentConditions = self.environment.getAirProperties(
Vector(0, 0, 200)) # m
self.rocketState1 = RigidBodyState(
Vector(0, 0, 200), Vector(0, 0, 200),
Quaternion(Vector(0, 0, 1), 0),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.rocketState2 = RigidBodyState(
Vector(0, 0, 200), Vector(0, 0, 200),
Quaternion(Vector(1, 0, 0), math.radians(2)),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.rocketState3 = RigidBodyState(
Vector(0, 0, 200), Vector(0, 0, 500),
Quaternion(Vector(1, 0, 0), math.radians(2)),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
def test_getConvertedVelocity(self):
state3 = RigidBodyState(
Vector(0, 0, 200), Vector(0, 0, 200),
Quaternion(Vector(1, 0, 0), 0.34906585),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
convertedVelocity = self.interface.getConvertedVelocity(state3)
self.assertAlmostEqual(convertedVelocity.X, state3.velocity.X)
self.assertAlmostEqual(convertedVelocity.Y, state3.velocity.Y)
self.assertAlmostEqual(convertedVelocity.Z, -state3.velocity.Z)
def setUp(self):
removeLogger()
self.dummyVelocity1 = Vector(0, 0, 50)
self.dummyVelocity2 = Vector(1, 0, 20)
self.dummyVelocity3 = Vector(0, 0, -100)
self.dummyOrientation1 = Quaternion(Vector(1, 0, 0), math.radians(2))
self.dummyOrientation2 = Quaternion(Vector(1, 0, 0), math.radians(-2))
self.dummyOrientation3 = Quaternion(Vector(0, 1, 0), math.radians(2))
self.dummyOrientation4 = Quaternion(Vector(1, 0, 0), 0)
self.dummyOrientation5 = Quaternion(Vector(1, 1, 0), math.radians(2))
self.dummyOrientation6 = Quaternion(Vector(1, 0, 0), math.radians(90))
self.environment = Environment(silent=True)
self.currentConditions = self.environment.getAirProperties(
Vector(0, 0, 200))
self.rocketState1 = RigidBodyState(
Vector(0, 0, 200), Vector(0, 0, 200),
Quaternion(Vector(0, 0, 1), 0),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.rocketState3 = RigidBodyState(
Vector(0, 0, 200), Vector(0, 0, 500),
Quaternion(Vector(1, 0, 0), math.radians(2)),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.rocketState4 = RigidBodyState(
Vector(0, 0, 200), Vector(0, 0, -200),
Quaternion(Vector(1, 0, 0), math.radians(180)),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.rocketState8 = RigidBodyState(
Vector(0, 0, 200), Vector(20.04, -0.12, -52.78),
Quaternion(Vector(0, 1, 0), math.radians(90)),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.correctDynamicPressure1 = self.currentConditions.Density * self.rocketState1.velocity.length(
)**2 / 2
self.correctDynamicPressure2 = self.currentConditions.Density * self.rocketState3.velocity.length(
)**2 / 2
def convertStateToENUFrame(self, globalFrameState: Union[RigidBodyState, RigidBodyState_3DoF]) -> Union[RigidBodyState, RigidBodyState_3DoF]:
altitude = self.earthModel.getAltitude(*globalFrameState.position)
position = Vector(0, 0, altitude) # Frame moves with the aircraft so x/y are always zero
inertialToENURotation = self.earthModel.getInertialToENUFrameRotation(*globalFrameState.position)
ENUToGlobalRotation = inertialToENURotation.conjugate()
velocity = ENUToGlobalRotation.rotate(globalFrameState.velocity)
try:
orientation = ENUToGlobalRotation * globalFrameState.orientation
angVel = ENUToGlobalRotation.rotate(globalFrameState.angularVelocity)
return RigidBodyState(position, velocity, orientation, angVel)
except:
return RigidBodyState_3DoF(position, velocity)
def test_interpolateRigidBodyStates(self):
vel1 = Vector(0, 1, 2)
vel2 = Vector(2, 3, 4)
pos1 = vel1
pos2 = Vector(4, 5, 6)
angVel1 = vel1
angVel2 = Vector(6, 7, 8)
quat1 = Quaternion(axisOfRotation=Vector(0, 0, 1), angle=0)
quat2 = Quaternion(axisOfRotation=Vector(0, 0, 1), angle=2)
rBS1 = RigidBodyState(pos1, vel1, quat1, angVel1)
rBS2 = RigidBodyState(pos2, vel2, quat2, angVel2)
rBS1p5 = interpolateRigidBodyStates(rBS1, rBS2, 0.5)
assertIterablesAlmostEqual(self, Vector(2, 3, 4), rBS1p5.position)
assertIterablesAlmostEqual(self, Vector(1, 2, 3), rBS1p5.velocity)
assertQuaternionsAlmostEqual(
self, rBS1p5.orientation,
Quaternion(axisOfRotation=Vector(0, 0, 1), angle=1))
assertIterablesAlmostEqual(self, rBS1p5.angularVelocity,
Vector(3, 4, 5))
def test_getConvertedQuaternion(self):
state3 = RigidBodyState(
Vector(0, 0, 200), Vector(0, 0, 200),
Quaternion(Vector(1, 0, 0), 0.34906585),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
convertedQuaternion = self.interface.getConvertedQuaternion(state3)
self.assertAlmostEqual(convertedQuaternion.Q[0], 0.9848077530468392, 4)
self.assertAlmostEqual(convertedQuaternion.Q[1], 0.17364817747052724,
4)
self.assertAlmostEqual(convertedQuaternion.Q[2], 0)
self.assertAlmostEqual(convertedQuaternion.Q[3], 0)
def setUp(self):
# Init rocket + environment
simRunner = Simulation(simDefinitionFilePath=
"MAPLEAF/Examples/Simulations/Recovery.mapleaf",
silent=True)
self.recoveryRocket = simRunner.createRocket()
self.rSys = self.recoveryRocket.recoverySystem
self.environment = self.recoveryRocket.environment
velDown = Vector(0, 0, -100)
velUp = Vector(0, 0, 100)
pos = Vector(0, 0, 500)
orientation = Quaternion(axisOfRotation=Vector(0, 0, 1), angle=0)
angVel = AngularVelocity(rotationVector=Vector(0, 0, 0))
self.descendingState = RigidBodyState(pos, velDown, orientation,
angVel)
self.descendingState_3DoF = RigidBodyState_3DoF(pos, velDown)
self.descendingTime = 2
self.ascendingState = RigidBodyState(pos, velUp, orientation, angVel)
self.ascendingState_3DoF = RigidBodyState_3DoF(pos, velUp)
self.ascendingTime = 2
def test_runControlLoop(self):
# Basic spin case
pos = Vector(0,0,0)
vel = Vector(0,0,0)
orientation = Quaternion(axisOfRotation=Vector(0,0,1), angle=-0.01)
angularVelocity = AngularVelocity(axisOfRotation=Vector(0,0,1), angularVel=0)
rigidBodyState = RigidBodyState(pos, vel, orientation, angularVelocity)
self.rocket.rigidBody.state = rigidBodyState
expectedAngleError = np.array([ 0, 0, 0.01 ])
dt = 1
ExpectedPIDCoeffs = [[ 4, 5, 6 ], [ 4, 5, 6 ], [ 7.5, 8.5, 9.5 ]]
ExpectedDer = expectedAngleError / dt
ExpectedIntegral = expectedAngleError * dt / 2
ExpectedM = []
for i in range(3):
moment = ExpectedPIDCoeffs[i][0]*expectedAngleError[i] + ExpectedPIDCoeffs[i][1]*ExpectedIntegral[i] + ExpectedPIDCoeffs[i][2]*ExpectedDer[i]
ExpectedM.append(moment)
# Replace keyFunctions with these ones that return fixed values
def fakeMach(*args):
# MachNum = 0.1
return 0.1
def fakeAltitude(*args):
# Altitude = 0.5
return 0.5
# Fake functions in ScheduledGainPIDMomentController
self.rocket.controlSystem.momentController.keyFunctionList = [ fakeMach, fakeAltitude ]
# Fake functions in TableInterpolatingActuatorController
self.rocket.controlSystem.controlledSystem.actuatorController.keyFunctionList = [ fakeMach, fakeAltitude ]
# Calculated in spreadsheet: test/linearFinDeflTestDataGenerator.ods
ExpectedFinDefl = [ 2.1625, 6.225, 10.2875, 14.35 ]
self.rocket.controlSystem.runControlLoopIfRequired(1, rigidBodyState, 'fakeEnvironment')
calculatedFinDefl = [ actuator.targetDeflection for actuator in self.rocket.controlSystem.controlledSystem.actuatorList ]
for i in range(3):
self.assertAlmostEqual(ExpectedFinDefl[i], calculatedFinDefl[i])
def test_bodyTubeDampingMoment(self):
simRunner = Simulation("MAPLEAF/Examples/Simulations/test3.mapleaf",
silent=True)
rocket = simRunner.createRocket()
bodyTube = rocket.stages[0].getComponentsOfType(BodyTube)[0]
# Create a rigid body state rotating at 2 rad/s about the x-axis
pos = Vector(0, 0, 0)
vel = Vector(0, 0, 0)
orientation = Quaternion(1, 0, 0, 0)
angVel = AngularVelocity(2, 0, 0)
xRotatingState = RigidBodyState(pos, vel, orientation, angVel)
envConditions = rocket.environment.getAirProperties(Vector(0, 0, 0))
#### CoR = Middle of the body tube ####
fakeRocketCG = Vector(0, 0, -1.762)
# Get computed (numerically-integrated) result
calculatedDampingMoment = bodyTube._computeLongitudinalDampingMoments(
xRotatingState, envConditions, fakeRocketCG, nSegments=100)
# Compute analytical result (Niskanen Eqn 3.58) * 2 for both halves of the body tube (above and below center of rotation)
expectedXDampingMoment = 2 * 0.275 * envConditions.Density * (
bodyTube.outerDiameter / 2) * 4
expectedTotalDampingMoment = Vector(-expectedXDampingMoment, 0, 0)
# Compare
assertVectorsAlmostEqual(self, calculatedDampingMoment,
expectedTotalDampingMoment, 4)
#### Case 2: CoR at End of Body Tube ####
fakeRocketCG = Vector(0, 0, -2.762)
# Get computed (numerically-integrated) result
calculatedDampingMoment = bodyTube._computeLongitudinalDampingMoments(
xRotatingState, envConditions, fakeRocketCG, nSegments=150)
# Factor of 16 from moving to a tube length of two (2^4 = 16), but dividing by two (one half instead of two)
expectedTotalDampingMoment *= 8
# Compare
assertVectorsAlmostEqual(self, calculatedDampingMoment,
expectedTotalDampingMoment, 4)
def uniformXMotionTest(self, integrationMethod):
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 = RigidBody(movingXState,
zeroForceFunc,
constInertiaFunc,
integrationMethod=integrationMethod,
simDefinition=self.simDefinition)
movingXBody.timeStep(1)
newPos = movingXBody.state.position
assertVectorsAlmostEqual(self, newPos, Vector(1, 0, 0))
def _initializeRigidBody(self):
#### Get initial kinematic state (in launch tower frame) ####
initPos = self.rocketDictReader.getVector("position")
initVel = self.rocketDictReader.getVector("velocity")
# Check whether precise initial orientation has been specified
rotationAxis = self.rocketDictReader.tryGetVector("rotationAxis", defaultValue=None)
if rotationAxis != None:
rotationAngle = math.radians(self.rocketDictReader.getFloat("rotationAngle"))
initOrientation = Quaternion(rotationAxis, rotationAngle)
else:
# Calculate initial orientation quaternion in launch tower frame
initialDirection = self.rocketDictReader.getVector("initialDirection").normalize()
angleFromVertical = Vector(0,0,1).angle(initialDirection)
rotationAxis = Vector(0,0,1).crossProduct(initialDirection)
initOrientation = Quaternion(rotationAxis, angleFromVertical)
initAngVel = AngularVelocity(rotationVector=self.rocketDictReader.getVector("angularVelocity"))
initState_launchTowerFrame = RigidBodyState(initPos, initVel, initOrientation, initAngVel)
# Convert to the global inertial frame
initState_globalInertialFrame = self.environment.convertInitialStateToGlobalFrame(initState_launchTowerFrame)
# Get desired time discretization method
timeDisc = self.rocketDictReader.getString("SimControl.timeDiscretization")
#TODO: Check for additional parameters to integrate - if they exist create a StatefulRigidBody + RocketState instead!
# Ask each rocket component whether it would like to add parameters to integrate after all the components have been initialized
discardDtCallback = None if (self.simRunner == None) else self.simRunner.discardForceLogsForPreviousTimeStep
#### Initialize the rigid body ####
self.rigidBody = RigidBody(
initState_globalInertialFrame,
self._getAppliedForce,
self.getInertia,
integrationMethod=timeDisc,
discardedTimeStepCallback=discardDtCallback,
simDefinition=self.simDefinition
)
def setUp(self):
pos1 = Vector(0, 0, 10)
vel1 = Vector(0, 0, -1)
pos2 = Vector(1, 2, 5)
vel2 = Vector(0, 0, -1)
t1 = 0.0
t2 = 1.0
state1 = RigidBodyState_3DoF(pos1, vel1)
state2 = RigidBodyState_3DoF(pos2, vel2)
orientation1 = Quaternion(axisOfRotation=Vector(0,0,1), angle=0)
angVel1 = Vector(0,0,0)
state3 = RigidBodyState(pos2, vel2, orientation1, angVel1)
flight = RocketFlight()
flight.rigidBodyStates = [ state1, state2, state3 ]
flight.times = [ t1, t2, 2.0 ]
flight.actuatorDefls = [ [2, 3, 4], [3, 4, 5], [ 4, 5, 6] ]
self.flight = flight
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 test_writePacket(self):
state1 = RigidBodyState(
Vector(0, 0, 250), Vector(0, 0, 250),
Quaternion(Vector(0, 0, 1), 0),
AngularVelocity(rotationVector=Vector(0, 0, 0)))
data = self.interface.getQuaternionByteArray(state1)
myPacket = packet(109)
myPacket.createPTByte(True, True, 3)
myPacket.writeData(data)
self.interface.writePacket(myPacket)
data = self.interface.getPositionByteArray(state1)
myPacket = packet(117)
myPacket.createPTByte(True, True, 4)
myPacket.writeData(data)
self.interface.writePacket(myPacket)
def uniformZRotationTest(self, integrationMethod):
rotatingZState = RigidBodyState(
self.zeroVec, self.zeroVec, self.zeroQuat,
AngularVelocity(axisOfRotation=Vector(0, 0, 1), angularVel=pi))
constInertia = Inertia(self.oneVec, self.zeroVec, 1)
def constInertiaFunc(*allArgs):
return constInertia
def constForceFunc(*allArgs):
return self.zeroForce
rotatingZBody = RigidBody(rotatingZState,
constForceFunc,
constInertiaFunc,
integrationMethod=integrationMethod,
simDefinition=self.simDefinition)
rotatingZBody.timeStep(1)
newOrientation = rotatingZBody.state.orientation
expectedOrientation = Quaternion(axisOfRotation=Vector(0, 0, 1),
angle=pi)
assertQuaternionsAlmostEqual(self, newOrientation, expectedOrientation)
def convertIntoGlobalFrame(self, launchTowerFrameState, lat, lon):
### Position ###
height = launchTowerFrameState.position.Z # ASL altitude
globalFramePosition = Vector(
*self.geodeticToCartesian(lat, lon, height))
### Velocity ###
# Find orientation of launch tower frame relative to global frame
launchTowerToGlobalFrame = self.getInertialToENUFrameRotation(
*globalFramePosition)
# Rotate velocity accordingly
rotatedVelocity = launchTowerToGlobalFrame.rotate(
launchTowerFrameState.velocity)
# Add earth's surface velocity
earthAngVel = Vector(0, 0, self.rotationRate)
velocityDueToEarthRotation = earthAngVel.crossProduct(
globalFramePosition)
globalFrameVelocity = rotatedVelocity + velocityDueToEarthRotation
try: # 6DoF Case
### Orientation ###
globalFrameOrientation = launchTowerToGlobalFrame * launchTowerFrameState.orientation
### Angular Velocity ###
# Angular velocity is defined in the vehicle's local frame, so the conversion needs to go the other way
earthAngVel_RocketFrame = globalFrameOrientation.conjugate(
).rotate(earthAngVel)
localFrame_adjustedAngVel = launchTowerFrameState.angularVelocity + earthAngVel_RocketFrame
return RigidBodyState(globalFramePosition, globalFrameVelocity,
globalFrameOrientation,
localFrame_adjustedAngVel)
except AttributeError: # 3DoF Case
return RigidBodyState_3DoF(globalFramePosition,
globalFrameVelocity)
def test_getDesiredMoments(self):
# Basic spin case
pos = Vector(0,0,0)
vel = Vector(0,0,0)
orientation = Quaternion(axisOfRotation=Vector(0,0,1), angle=0.12)
targetOrientation = Quaternion(axisOfRotation=Vector(0,0,1), angle=0)
angularVelocity = AngularVelocity(axisOfRotation=Vector(0,0,1), angularVel=0)
rigidBodyState = RigidBodyState(pos, vel, orientation, angularVelocity)
expectedAngleError = np.array([ 0, 0, -0.12 ])
dt = 1
ExpectedPIDCoeffs = [[ 4, 5, 6 ], [ 4, 5, 6 ], [ 7.5, 8.5, 9.5 ]]
ExpectedDer = expectedAngleError / dt
ExpectedIntegral = expectedAngleError * dt / 2
ExpectedM = []
for i in range(3):
moment = ExpectedPIDCoeffs[i][0]*expectedAngleError[i] + ExpectedPIDCoeffs[i][1]*ExpectedIntegral[i] + ExpectedPIDCoeffs[i][2]*ExpectedDer[i]
ExpectedM.append(moment)
# Replace keyFunctions with these ones that return fixed values
def fakeMach(*args):
# MachNum = 0.1
return 0.1
def fakeAltitude(*args):
# Altitude = 0.5
return 0.5
self.momentController.keyFunctionList = [ fakeMach, fakeAltitude ]
calculatedMoments = self.momentController.getDesiredMoments(rigidBodyState, "fakeEnvironemt", targetOrientation, 0, dt)
for i in range(3):
# print(calculatedMoments[i])
# print(ExpectedM[i])
self.assertAlmostEqual(calculatedMoments[i], ExpectedM[i])
