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_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())
class TestAngularVelocity(unittest.TestCase):
def setUp(self):
self.angVel = AngularVelocity(rotationVector=Vector(0, 0, 1))
self.angVel2 = AngularVelocity(rotationVector=Vector(0, 0.5, 0))
self.quat1 = (self.angVel + self.angVel2).toQuaternion()
def test_add(self):
result = self.angVel + self.angVel2
self.assertEqual(result,
AngularVelocity(rotationVector=Vector(0, 0.5, 1)))
def test_sub(self):
result = self.angVel - self.angVel2
self.assertEqual(result,
AngularVelocity(rotationVector=Vector(0, -0.5, 1)))
def test_initFromComp(self):
angVel = AngularVelocity(0, 2, 5)
self.assertEqual(Vector(0, 2, 5), angVel)
def test_ToQuat(self):
result = self.angVel.toQuaternion()
self.assertEqual(
result,
Quaternion(axisOfRotation=self.angVel, angle=self.angVel.length()))
def __init__(self,
position=None,
velocity=None,
orientation=None,
angularVelocity=None):
self.position = Vector(0, 0, 0) if (position is None) else position
self.velocity = Vector(0, 0, 0) if (velocity is None) else velocity
self.orientation = Quaternion(
1, 0, 0, 0) if (orientation is None) else orientation
self.angularVelocity = AngularVelocity(
0, 0, 0) if (angularVelocity is None) else angularVelocity
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_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 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 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_forcesSymmetricForSymmetricRocket(self):
self.rocket2.rigidBody.state.angularVelocity = AngularVelocity(0, 0, 0)
self.rocket2.rigidBody.state.velocity = Vector(
0, 10, 100) #5.7 degree angle of attack
self.rocket2.rigidBody.state.orientation = Quaternion(
axisOfRotation=Vector(0, 0, 1), angle=0)
appliedForce = self.rocket2._getAppliedForce(
0, self.rocket2.rigidBody.state)
self.assertAlmostEqual(appliedForce.moment.Y, 0)
self.assertAlmostEqual(appliedForce.moment.Z, 0)
self.rocket2.rigidBody.state.velocity = Vector(
0, 100, 100) #45 degree angle of attack
self.rocket2.rigidBody.state.orientation = Quaternion(
axisOfRotation=Vector(0, 0, 1), angle=0)
appliedForce = self.rocket2._getAppliedForce(
0, self.rocket2.rigidBody.state)
self.assertAlmostEqual(appliedForce.moment.Y, 0)
self.assertAlmostEqual(appliedForce.moment.Z, 0)
self.assertAlmostEqual(appliedForce.moment.Z, 0)
self.rocket2.rigidBody.state.velocity = Vector(
0, 500, 500) #45 degree angle of attack
self.rocket2.rigidBody.state.orientation = Quaternion(
axisOfRotation=Vector(0, 0, 1), angle=0)
appliedForce = self.rocket2._getAppliedForce(
0, self.rocket2.rigidBody.state)
self.assertAlmostEqual(appliedForce.moment.Y, 0)
self.assertAlmostEqual(appliedForce.moment.Z, 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 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_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 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):
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_MultiplyRigidBodyState(self):
iRBS3 = self.iRBS2 * 0.5
self.assertEqual(iRBS3.position, Vector(0.5, 0, 0))
self.assertEqual(iRBS3.velocity, Vector(1, 0, 0))
assertQuaternionsAlmostEqual(self,
iRBS3.orientation,
Quaternion(axisOfRotation=Vector(1, 0, 0),
angle=pi / 4),
n=14)
self.nearlyEqualAngVel(
iRBS3.angularVelocity,
AngularVelocity(axisOfRotation=Vector(1, 0, 0), angularVel=pi / 2))
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 setUp(self):
simDef = SimDefinition("MAPLEAF/Examples/Simulations/test3.mapleaf", silent=True)
rocketDictReader = SubDictReader("Rocket", simDef)
self.rocket = Rocket(rocketDictReader, silent=True)
simDef = SimDefinition("MAPLEAF/Examples/Simulations/test6.mapleaf", silent=True)
rocketDictReader = SubDictReader("Rocket", simDef)
self.rocket2 = Rocket(rocketDictReader, silent=True)
self.finSet1 = self.rocket2.stages[0].getComponentsOfType(FinSet)[0]
self.finSet2 = self.rocket2.stages[0].getComponentsOfType(FinSet)[1]
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.dummyAngularVelocity1 = AngularVelocity(rotationVector = Vector(0,0,0))
self.dummyAngularVelocity2 = AngularVelocity(rotationVector = Vector(0,0,1))
self.dummyAngularVelocity3 = AngularVelocity(rotationVector = Vector(0,0,-1))
self.dummyAngularVelocity4 = AngularVelocity(rotationVector = Vector(1,0,0))
self.dummyAngularVelocity5 = AngularVelocity(rotationVector = Vector(-1,0,0))
self.dummyAngularVelocity6 = AngularVelocity(rotationVector = Vector(0,1,0))
self.dummyAngularVelocity7 = AngularVelocity(rotationVector = Vector(0,-1,0))
self.dummyAngularVelocity8 = AngularVelocity(rotationVector = Vector(1,1,0))
self.currentConditions = self.rocket2.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)))
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.rocketState5 = RigidBodyState(Vector(0, 0, 200), Vector(0, 0, -200), Quaternion(Vector(1, 0, 0), math.radians(178)), AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.rocketState6 = RigidBodyState(Vector(0, 0, 200), Vector(0, 0, -200), Quaternion(Vector(1, 0, 0), math.radians(182)), AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.rocketState7 = RigidBodyState(Vector(0, 0, 200), Vector(0, 0, -200), Quaternion(Vector(1, 0, 0), math.radians(90)), 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.rocketState9 = RigidBodyState(Vector(0, 0, 200), Vector(0, 0, 200), Quaternion(Vector(1, 0, 0), math.radians(-2)), AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.rocketState10 = RigidBodyState(Vector(0, 0, 200), Vector(0, 0, 200), Quaternion(Vector(0, 0, 1), 0), AngularVelocity(rotationVector=Vector(0, 0, 1)))
self.rocketState11 = RigidBodyState(Vector(0, 0, 200), Vector(0, 0, 200), Quaternion(Vector(0, 1, 0), math.radians(2)), AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.rocketState12 = RigidBodyState(Vector(0, 0, 200), Vector(0, 0, 200), Quaternion(Vector(0, 1, 0), math.radians(-2)), AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.rocketState13 = RigidBodyState(Vector(0, 0, 200), Vector(0, 0, 500), Quaternion(Vector(1, 0, 0), math.radians(4)), AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.rocketState14 = RigidBodyState(Vector(0, 0, 200), Vector(0, 0, 500), Quaternion(Vector(1, 0, 0), math.radians(6)), AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.rocketState15 = RigidBodyState(Vector(0, 0, 200), Vector(0, 0, 500), Quaternion(Vector(1, 0, 0), math.radians(8)), AngularVelocity(rotationVector=Vector(0, 0, 0)))
self.rocketState16 = RigidBodyState(Vector(0, 0, 200), Vector(0, 0, 344), Quaternion(Vector(1, 0, 0), math.radians(0)), AngularVelocity(rotationVector=Vector(0, 0, 0)))
def getRigidBodyStateDerivative(self, time, state):
# Forces are expected to be calculated in a body frame, where each coordinate axis is aligned with a pricipal axis
appliedForce_localFrame = self.forceFunc(time, state)
inertia = self.inertiaFunc(time, state)
# Get CG velocity relative to body geometry
dt = 1e-8
inertia2 = self.inertiaFunc(time + dt, state)
CGVel_local = (inertia2.CG - inertia.CG) / dt
CGVel_global = state.orientation.rotate(CGVel_local)
### Translation - calculated in global frame ###
#Convert from local to global frame
fVec_global = state.orientation.rotate(appliedForce_localFrame.force)
linAccel_global = fVec_global / inertia.mass
### Rotation - calculated in local frame (Euler equations) ###
# convert angular velocity to global frame - to be added to orientation
angVel_global = AngularVelocity(
*state.orientation.rotate(state.angularVelocity)
) # Will be transformed into a quaternion once multiplied by a timestep
# Calc angular acceleration (in local frame)
moi = inertia.MOI
dAngVelDtX = (appliedForce_localFrame.moment.X +
(moi.Y - moi.Z) * state.angularVelocity.Y *
state.angularVelocity.Z) / moi.X
dAngVelDtY = (appliedForce_localFrame.moment.Y +
(moi.Z - moi.X) * state.angularVelocity.Z *
state.angularVelocity.X) / moi.Y
dAngVelDtZ = (appliedForce_localFrame.moment.Z +
(moi.X - moi.Y) * state.angularVelocity.X *
state.angularVelocity.Y) / moi.Z
dAngVelDt = AngularVelocity(dAngVelDtX, dAngVelDtY, dAngVelDtZ)
return RigidBodyStateDerivative(state.velocity + CGVel_global,
linAccel_global, angVel_global,
dAngVelDt)
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 test_nonPrincipalAxisRotation(self):
initAngVel = AngularVelocity(rotationVector=Vector(1, 0, 1))
rotatingZState = RigidBodyState(self.zeroVec, self.zeroVec,
self.zeroQuat, initAngVel)
constForce = ForceMomentSystem(self.zeroVec)
def constForceFunc(*allArgs):
return constForce
constInertia = Inertia(Vector(2, 2, 8), self.zeroVec, 1)
def constInertiaFunc(*allArgs):
return constInertia
rotatingZBody = RigidBody(rotatingZState,
constForceFunc,
constInertiaFunc,
integrationMethod="RK4",
simDefinition=self.simDefinition)
dt = 0.01
nTimeSteps = 10
totalTime = dt * nTimeSteps
for i in range(nTimeSteps):
rotatingZBody.timeStep(dt)
finalAngVel = rotatingZBody.state.angularVelocity
omega = (constInertia.MOI.Z - constInertia.MOI.X) / constInertia.MOI.X
expectedAngVel = AngularVelocity(
rotationVector=Vector(cos(omega * totalTime), sin(omega *
totalTime), 1))
assertAngVelAlmostEqual(self, finalAngVel, expectedAngVel)
def setUp(self):
self.zeroVec = Vector(0, 0, 0)
self.zeroQuat = Quaternion(axisOfRotation=Vector(1, 0, 0), angle=0)
self.zeroAngVel = AngularVelocity(axisOfRotation=Vector(1, 0, 0),
angularVel=0)
self.zeroForce = ForceMomentSystem(self.zeroVec)
self.oneVec = Vector(1, 1, 1)
self.simDefinition.setValue(
"SimControl.TimeStepAdaptation.minTimeStep", "1")
#TODO: Get some tests for the higher order adaptive methods in here
self.integrationMethods = [
"Euler", "RK2Heun", "RK2Midpoint", "RK4", "RK12Adaptive"
]
self.constInertia = Inertia(self.oneVec, self.zeroVec, 1)
def __init__(self,
rigidBodyState,
forceParam,
inertiaParam,
integrationMethod="Euler",
discardedTimeStepCallback=None,
simDefinition=None):
''' Just calls RigidBodyState_3DoF constructor '''
super().__init__(rigidBodyState,
forceParam,
inertiaParam,
integrationMethod=integrationMethod,
simDefinition=simDefinition,
discardedTimeStepCallback=discardedTimeStepCallback)
self.lastStateDerivative = RigidBodyStateDerivative(
rigidBodyState.velocity, Vector(0, 0, 0),
rigidBodyState.angularVelocity, AngularVelocity(0, 0, 0))
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 _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 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 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])
def convertInitialStateToGlobalFrame(self, initialStateInLaunchTowerFrame):
'''
Used to convert the rocket's initial kinematic state from the launch tower frame (in which it is defined) and
into the global inertia frame
Takes a rigid body state defined in the launch tower frame and converts it to the global frame
For a flat earth model, this just adjusts for the altitude of the launch site,
but for a rotating earth model, this modifies every part of the rigid body state:
1. Position is redefined relative to the center of the earth (Acoording to lat/lon)
2. The velocity of earth's rotation is added to the velocity
3. The orientation is redefined relative to the global frame
4. The angular velocity of the earth is added to the rocket's initial state
.. note:: If a launch rail is being used, set the launch-tower-frame angular velocity to zero before doing the conversion
'''
# In all cases, first redefine the present state relative to sea level
initialStateInLaunchTowerFrame.position += Vector(0,0,self.launchSiteElevation)
# Set launch-tower-frame angular velocity to zero if using a launch rail
if self.launchRail != None:
initialStateInLaunchTowerFrame.angularVelocity = AngularVelocity(0,0,0)
# Call the current earth model's conversion function
return self.earthModel.convertIntoGlobalFrame(initialStateInLaunchTowerFrame, self.launchSiteLatitude, self.launchSiteLongitude)
