def test_Equality(self):
inertia1 = Inertia(Vector(1, 2, 3), Vector(4, 5, 6), 7)
inertia2 = Inertia(Vector(1, 2, 3), Vector(4, 5, 6), 7)
self.assertEqual(inertia1, inertia2)
inertia3 = Inertia(Vector(2, 1, 3), Vector(4, 5, 6), 7)
self.assertNotEqual(inertia1, inertia3)
def test_combineInertiasAboutPoint_TShapedBlock(self):
# http://adaptivemap.ma.psu.edu/websites/A2_moment_intergrals/parallel_axis_theorem/pdf/ParallelAxis_WorkedProblem2.pdf
# T-shaped block
# Top block (1) dimensions are (4,2,0)
# Bottom block (2) dimensions are (2,4,0)
Ixx1 = 1 / 12 * 2 * 4**3
Ixx2 = 1 / 12 * 4 * 2**3
Iyy1 = Ixx2
Iyy2 = Ixx1
MOI1 = Vector(Ixx1, Iyy1, 0)
centroid1 = Vector(0, 3, 0)
mass1 = 8
inertia1 = Inertia(MOI1, centroid1, mass1, centroid1)
MOI2 = Vector(Ixx2, Iyy2, 0)
centroid2 = Vector(0, 0, 0)
mass2 = 8
inertia2 = Inertia(MOI2, centroid2, mass2, centroid2)
# Add them up at the overall centroid
overallCentroid = Vector(0, 1.5, 0)
combinedInertia = inertia1.combineInertiasAboutPoint([inertia2],
overallCentroid)
self.assertAlmostEqual(combinedInertia.MOI.X, 49.333333333333)
self.assertAlmostEqual(combinedInertia.mass, 16)
self.assertAlmostEqual(combinedInertia.CG, overallCentroid)
self.assertAlmostEqual(combinedInertia.MOICentroidLocation,
overallCentroid)
def test_InertiaAddition(self):
mCG1 = Inertia(Vector(0, 0, 0), Vector(0, 0, 2), 10)
mCG2 = Inertia(Vector(0, 0, 0), Vector(0, 0, 0), 20)
m3 = mCG1.combineInertiasAboutPoint([mCG2], Vector(0, 0, 0))
self.assertEqual(m3.mass, 30)
self.assertAlmostEqual(m3.CG.Z, 0.6666666666666)
def test_compositeObjectOfCompositeObjects(self):
ballMOI = Vector(0.16, 0.16, 0.16)
ballCentroid1 = Vector(-0.4, 0, 0)
ballCentroid2 = Vector(0.4, 0, 0)
ballCentroid3 = Vector(1.2, 0, 0)
ballCentroid4 = Vector(2.0, 0, 0)
ball1Inertia = Inertia(ballMOI, ballCentroid1, 10)
ball2Inertia = Inertia(ballMOI, ballCentroid2, 20)
ball3Inertia = Inertia(ballMOI, ballCentroid3, 30)
ball4Inertia = Inertia(ballMOI, ballCentroid4, 40)
Ball1 = FixedMassForCompositeObjectTest(ball1Inertia)
Ball2 = FixedMassForCompositeObjectTest(ball2Inertia)
Ball3 = FixedMassForCompositeObjectTest(ball3Inertia)
Ball4 = FixedMassForCompositeObjectTest(ball4Inertia)
innerCompObject1 = CompositeObject([Ball1, Ball2])
innerCompObject2 = CompositeObject([Ball3, Ball4])
completeCompositeObject = CompositeObject([Ball1, Ball2, Ball3, Ball4])
outerCompObject = CompositeObject([innerCompObject1, innerCompObject2])
# Check whether we get the same results from outer and complete composite objects
innerMOI = outerCompObject.getInertia(0, None).MOI
completeMOI = completeCompositeObject.getInertia(0, None).MOI
assertIterablesAlmostEqual(self, innerMOI, completeMOI)
def test_MotorGetFuelInertia(self):
motor = self.motorRocket.stages[0].getComponentsOfType(
TabulatedMotor)[0]
assertInertiasAlmostEqual(
self, motor._getFuelInertia(0),
Inertia(Vector(0.15, 0.15, 0.02), Vector(0, 0, 3),
1.4985000000000002))
assertInertiasAlmostEqual(self, motor._getFuelInertia(5),
Inertia(Vector(0, 0, 0), Vector(0, 0, 3), 0))
def test_MotorGetOxInertia(self):
motor = self.motorRocket.stages[0].getComponentsOfType(
TabulatedMotor)[0]
assertInertiasAlmostEqual(
self, motor._getOxInertia(0),
Inertia(Vector(1, 1, 0.1), Vector(0, 0, 2), 9.99))
assertInertiasAlmostEqual(
self, motor._getOxInertia(5),
Inertia(Vector(0, 0, 0), Vector(0, 0, 2.5), 0.0))
def test_getInertia(self):
noseconeInertia = Inertia(Vector(0.001,0.001,0.001), Vector(0,0,-0.2), mass=5)
bodytubeInertia = Inertia(Vector(0.001,0.001,0.001), Vector(0,0,-1), mass=50)
generalInertia = Inertia(Vector(0.001,0.001,0.001), Vector(0,0,0.0), mass=100)
state = self.rocket.rigidBody.state
self.almostEqualVectors(self.nosecone.getInertia(0, state).CG, noseconeInertia.CG)
self.assertEqual(self.nosecone.getInertia(0, state).mass, noseconeInertia.mass)
self.almostEqualVectors(self.bodytube.getInertia(0, state).CG, bodytubeInertia.CG)
self.assertEqual(self.bodytube.getInertia(0, state).mass, bodytubeInertia.mass)
self.almostEqualVectors(self.fixedMass.getInertia(0, state).CG, generalInertia.CG)
self.assertEqual(self.fixedMass.getInertia(0, state).mass, generalInertia.mass)
def test_combineInertiasAboutPoint_SlenderRod(self):
# Test transforming from center to endpoint of slender rod
rodMOI = Vector(1 / 12, 1 / 12, 0)
rodCentroid = Vector(0, 0, 0)
rodMass = 1
rodCG = Vector(0, 0, 0)
centroidInertia = Inertia(rodMOI, rodCentroid, rodMass, rodCG)
rodEndPoint = Vector(0, 0, -0.5)
endPointInertia = centroidInertia.combineInertiasAboutPoint(
[], rodEndPoint)
assertIterablesAlmostEqual(self, endPointInertia.MOI,
Vector(1 / 3, 1 / 3, 0))
def test_UniformXMotionAndScalarIntegration(self):
movingXState = RigidBodyState(self.zeroVec, Vector(1, 0, 0),
self.zeroQuat, self.zeroAngVel)
constInertia = Inertia(self.oneVec, self.zeroVec, 1)
def constInertiaFunc(*allArgs):
return constInertia
def zeroForceFunc(*allArgs):
return self.zeroForce
movingXBody = StatefulRigidBody(movingXState,
zeroForceFunc,
constInertiaFunc,
integrationMethod="RK4",
simDefinition=self.simDefinition)
movingXBody.addStateVariable("scalarVar", 3, sampleDerivative)
movingXBody.timeStep(0.2)
# Check that the rigid body state has been integrated correctly
newPos = movingXBody.state[0].position
assertVectorsAlmostEqual(self, newPos, Vector(0.2, 0, 0))
# Check that the scalar function has been integrated correctly
finalVal = movingXBody.state[1]
self.assertAlmostEqual(finalVal, 2.0112)
def test_dualUniformXMotion(self):
movingXState = RigidBodyState(self.zeroVec, Vector(1, 0, 0),
self.zeroQuat, self.zeroAngVel)
constInertia = Inertia(self.oneVec, self.zeroVec, 1)
def constInertiaFunc(*allArgs):
return constInertia
def zeroForceFunc(*allArgs):
return self.zeroForce
movingXBody = StatefulRigidBody(movingXState,
zeroForceFunc,
constInertiaFunc,
simDefinition=self.simDefinition)
# Add the same rigid body state and state derivative function as a second state variable to be integrated
movingXBody.addStateVariable("secondRigidBodyState", movingXState,
movingXBody.getRigidBodyStateDerivative)
movingXBody.timeStep(1)
# Check that both rigid body states have been integrated correctly
newPos = movingXBody.state[0].position
newPos2 = movingXBody.state[1].position
assertVectorsAlmostEqual(self, newPos, Vector(1, 0, 0))
assertVectorsAlmostEqual(self, newPos2, Vector(1, 0, 0))
def test_MotorInertia(self):
motorRocket = self.motorRocket
motor = motorRocket.stages[0].getComponentsOfType(TabulatedMotor)[0]
beginningInertia = Inertia(Vector(0.15, 0.15, 0.02), Vector(
0, 0, 3), 1.4985000000000002) + Inertia(Vector(1, 1, 0.1),
Vector(0, 0, 2), 9.99)
finalInertia = Inertia(Vector(0, 0, 0), Vector(0, 0, 3), 0) + Inertia(
Vector(0, 0, 0), Vector(0, 0, 0), 0.0)
assertInertiasAlmostEqual(
self, motor.getInertia(0, motorRocket.rigidBody.state),
beginningInertia)
assertInertiasAlmostEqual(
self, motor.getInertia(5, motorRocket.rigidBody.state),
finalInertia)
def _ensureBaseDragIsAccountedFor(self):
''' If no BoatTail exists at the bottom of the rocket, adds a zero-length boat tail. This is necessary b/c Boat Tail aero-functions are the ones that account for base drag '''
boatTailComponentAtBottomOfRocket = False
bottomStage = self.stages[-1]
for comp in reversed(bottomStage.components):
if isinstance(comp, BodyComponent):
if isinstance(comp, BoatTail):
boatTailComponentAtBottomOfRocket = True
break
if not boatTailComponentAtBottomOfRocket and self.addZeroLengthBoatTailsToAccountForBaseDrag:
if not self.silent:
print("Adding zero-length BoatTail to the bottom of current bottom stage ({}) to account for base drag".format(bottomStage.name))
# Create a zero-length, zero-mass boat tail to account for base drag
zeroInertia = Inertia(Vector(0,0,0), Vector(0,0,0), 0)
diameter = self.maxDiameter # TODO: Get the actual bottom-body-tube diameter from a future Stage.getRadius function
length = 0
position = bottomStage.getBottomInterfaceLocation()
boatTail = BoatTail(
diameter,
diameter,
length,
position,
zeroInertia,
self,
bottomStage,
"Auto-AddedZeroLengthBoatTail",
self.surfaceRoughness
)
initializeForceLogging(boatTail, "FakeRocketName.Auto-AddedZeroLengthBoatTail", self)
bottomStage.components.append(boatTail)
def __init__(self, componentDictReader, rocket, stage):
self.rocket = rocket
self.stage = stage
self.componentDictReader = componentDictReader
self.name = componentDictReader.getDictName()
mass = componentDictReader.getFloat("mass")
# Position in simulation definition is relative to stage position
self.position = componentDictReader.getVector(
"position"
) + stage.position # Store position relative to nosecone here
# CG in simulation definition is relative to component position
cg = componentDictReader.getVector(
"cg"
) + self.position # Store cg location relative to nosecone here
try:
MOI = componentDictReader.getVector("MOI")
except:
MOI = Vector(mass * 0.01, mass * 0.01,
mass * 0.01) # Avoid having zero moments of inertia
self.inertia = Inertia(MOI, cg, mass)
self.zeroForce = ForceMomentSystem(Vector(0, 0, 0))
class AeroForce(RocketComponent):
''' A zero-Inertia component with constant aerodynamic coefficients '''
# Object is just a force, inertia is zero
inertia = Inertia(Vector(0, 0, 0), Vector(0, 0, 0), 0)
def __init__(self, componentDictReader, rocket, stage):
self.componentDictReader = componentDictReader
self.rocket = rocket
self.stage = stage
self.name = componentDictReader.getDictName()
self.position = componentDictReader.getVector("position")
self.Aref = componentDictReader.getFloat("Aref")
self.Lref = componentDictReader.getFloat("Lref")
Cd = componentDictReader.getFloat("Cd")
Cl = componentDictReader.getFloat("Cl")
momentCoeffs = componentDictReader.getVector("momentCoeffs")
self.aeroCoeffs = [Cd, Cl, *momentCoeffs]
def getInertia(self, time, state):
return self.inertia
def getAppliedForce(self, state, time, environment, rocketCG):
return AeroFunctions.forceFromCoefficients(state, environment,
*self.aeroCoeffs,
self.position, self.Aref,
self.Lref)
def _getFuelInertia(self, timeSinceIgnition):
if self.initialFuelWeight == 0:
return Inertia(Vector(0, 0, 0), Vector(0, 0, 0), 0)
#See comments in _getOxInertia()
fuelWeight = linInterp(self.times, self.fuelWeights, timeSinceIgnition)
fuelBurnedFrac = 1 - (fuelWeight / self.initialFuelWeight)
fuelCG_Z = self.initFuelCG_Z * (
1 - fuelBurnedFrac) + self.finalFuelCG_Z * fuelBurnedFrac
fuelCG = Vector(0, 0, fuelCG_Z)
fuelMOI = linInterp(self.times, self.fuelMOIs, timeSinceIgnition)
return Inertia(fuelMOI, fuelCG, fuelWeight)
def getInertia(self, time, rocketState):
mass = 5 + rocketState.tankLevel * 4.56 # Fixed Mass + fluid mass
MOI = Vector(mass, mass, mass * 0.05) # Related to current mass
CGz = -3 + rocketState.tankLevel # Moves depending on current tank level
CG = Vector(0, 0, CGz)
return Inertia(MOI, CG, mass)
def test_getFixedForceInertia(self):
simDef = SimDefinition("MAPLEAF/Examples/Simulations/FixedForce.mapleaf", silent=True)
rocketDictReader = SubDictReader("Rocket", simDef)
rocket = Rocket(rocketDictReader, silent=True)
fixedForce = rocket.stages[0].getComponentsOfType(FixedForce)[0]
inertia = fixedForce.getInertia(0, "fakeState")
expectedInertia = Inertia(Vector(0,0,0), Vector(0,0,0), 0)
self.assertEqual(inertia, expectedInertia)
def _getOxInertia(self, timeSinceIgnition):
if self.initialOxidizerWeight == 0:
return Inertia(Vector(0, 0, 0), Vector(0, 0, 0), 0)
oxWeight = linInterp(self.times, self.oxWeights, timeSinceIgnition)
# Find fraction of oxidizer burned
oxBurnedFrac = 1 - (oxWeight / self.initialOxidizerWeight)
#Linearly interpolate CG location based on fraction of oxidizer burned
oxCG_Z = self.initOxCG_Z * (
1 - oxBurnedFrac) + self.finalOxCG_Z * oxBurnedFrac
#TODO: Allow motor(s) to be defined off-axis
oxCG = Vector(0, 0, oxCG_Z)
# Get MOI
oxMOI = linInterp(self.times, self.oxMOIs, timeSinceIgnition)
return Inertia(oxMOI, oxCG, oxWeight)
def test_MotorMOI(self):
motorRocket = self.motorRocket
motor = self.motorRocket.stages[0].getComponentsOfType(
TabulatedMotor)[0]
### Oxidizer MOI Tests ###
def getOxMOI(time):
return motor._getOxInertia(time).MOI
ActualOxMOI = getOxMOI(0)
expectedOxMOI = Vector(1.0, 1.0, 0.1)
assertIterablesAlmostEqual(self, ActualOxMOI, expectedOxMOI)
ActualOxMOI = getOxMOI(2.505)
expectedOxMOI = Vector(0.5, 0.5, 0.05)
assertIterablesAlmostEqual(self, ActualOxMOI, expectedOxMOI)
ActualOxMOI = getOxMOI(5.0)
expectedOxMOI = Vector(0, 0, 0)
assertIterablesAlmostEqual(self, ActualOxMOI, expectedOxMOI)
ActualOxMOI = getOxMOI(10)
expectedOxMOI = Vector(0, 0, 0)
assertIterablesAlmostEqual(self, ActualOxMOI, expectedOxMOI)
### Fuel MOI Tests ###
def getFuelMOI(time):
return motor._getFuelInertia(time).MOI
ActualFuelMOI = getFuelMOI(0)
expectedFuelMOI = Vector(0.15, 0.15, 0.02)
assertIterablesAlmostEqual(self, ActualFuelMOI, expectedFuelMOI)
ActualFuelMOI = getFuelMOI(2.505)
expectedFuelMOI = Vector(0.075, 0.075, 0.01)
assertIterablesAlmostEqual(self, ActualFuelMOI, expectedFuelMOI)
ActualFuelMOI = getFuelMOI(5)
expectedFuelMOI = Vector(0, 0, 0)
assertIterablesAlmostEqual(self, ActualFuelMOI, expectedFuelMOI)
ActualFuelMOI = getFuelMOI(10)
expectedFuelMOI = Vector(0, 0, 0)
assertIterablesAlmostEqual(self, ActualFuelMOI, expectedFuelMOI)
simDef = SimDefinition("MAPLEAF/Examples/Simulations/Jake1.mapleaf")
rocketDictReader = SubDictReader("Rocket", simDef)
jakeRocket = Rocket(rocketDictReader)
motor = jakeRocket.stages[0].motor
motorInertia = motor.getInertia(3, jakeRocket.rigidBody.state)
expectedInertiaAfterBurnout = Inertia(Vector(0, 0, 0), Vector(0, 0, 0),
0.0)
assertInertiasAlmostEqual(self, motorInertia,
expectedInertiaAfterBurnout)
def test_compositeObject_Dumbbell(self):
# http://adaptivemap.ma.psu.edu/websites/A2_moment_intergrals/parallel_axis_theorem/pdf/ParallelAxis_WorkedProblem3.pdf
# Two 40kg 0.2m diameter weights at each end
# One 20kg, 0.6m long bar between them
# (0,0,0) is at the center of the rod between the dumbbells
ballMOI = Vector(0.16, 0.16, 0.16)
ballCentroid1 = Vector(-0.4, 0, 0)
ballCentroid2 = Vector(0.4, 0, 0)
ballMass = 40
ball1Inertia = Inertia(ballMOI, ballCentroid1, ballMass, ballCentroid1)
ball1 = FixedMassForCompositeObjectTest(ball1Inertia)
ball2Inertia = Inertia(ballMOI, ballCentroid2, ballMass, ballCentroid2)
ball2 = FixedMassForCompositeObjectTest(ball2Inertia)
rodMOI = Vector(0, 0.6, 0.6)
rodCentroid = Vector(0, 0, 0)
rodMass = 20
rodInertia = Inertia(rodMOI, rodCentroid, rodMass, rodCentroid)
rod = FixedMassForCompositeObjectTest(rodInertia)
# Create composite object
dumbbell = CompositeObject([ball1, ball2, rod])
# Check that fixed mass caching is working properly
self.assertEqual(len(dumbbell.fixedMassComponents), 3)
# Get inertia and check it
combinedInertia = dumbbell.getInertia(0, None)
self.assertAlmostEqual(combinedInertia.MOI.Y, 13.72)
self.assertAlmostEqual(combinedInertia.MOI.Z, 13.72)
self.assertAlmostEqual(combinedInertia.MOI.X, 0.32)
self.assertAlmostEqual(combinedInertia.mass, 100)
self.assertAlmostEqual(combinedInertia.CG, Vector(0, 0, 0))
# test the getMass function
mass = dumbbell.getMass(0, None)
self.assertAlmostEqual(100, mass)
# test the getCG function
CG = dumbbell.getCG(0, None)
self.assertAlmostEqual(CG, Vector(0, 0, 0))
def test_combineInertias_Dumbbell(self):
# http://adaptivemap.ma.psu.edu/websites/A2_moment_intergrals/parallel_axis_theorem/pdf/ParallelAxis_WorkedProblem3.pdf
# Two 40kg 0.2m diameter weights at each end
# One 20kg, 0.6m long bar between them
# (0,0,0) is at the center of the rod between the dumbbells
ballMOI = Vector(0.16, 0.16, 0.16)
ballCentroid1 = Vector(-0.4, 0, 0)
ballCentroid2 = Vector(0.4, 0, 0)
ballMass = 40
ball1Inertia = Inertia(ballMOI, ballCentroid1, ballMass, ballCentroid1)
ball2Inertia = Inertia(ballMOI, ballCentroid2, ballMass, ballCentroid2)
rodMOI = Vector(0, 0.6, 0.6)
rodCentroid = Vector(0, 0, 0)
rodMass = 20
rodInertia = Inertia(rodMOI, rodCentroid, rodMass, rodCentroid)
combinedInertia = ball1Inertia.combineInertias(
[ball2Inertia, rodInertia])
self.assertAlmostEqual(combinedInertia.MOI.Y, 13.72)
def _initializeFixedMassInertia(self):
if len(self.fixedMassComponents) > 0:
inertiasList = []
for fixedMassComponent in self.fixedMassComponents:
inertiasList.append(fixedMassComponent.getInertia(0, None))
self.fixedMassInertiaComponent = inertiasList[0].combineInertias(
inertiasList)
self.fixedMassMassComponent = self.fixedMassInertiaComponent.mass
else:
self.fixedMassInertiaComponent = Inertia(Vector(0, 0, 0),
Vector(0, 0, 0), 0)
self.fixedMassMassComponent = 0
def __init__(self, componentDictReader, rocket, stage):
''' A Zero-inertia component that applies a constant ForceMomentSystem to the rocket '''
self.componentDictReader = componentDictReader
self.rocket = rocket
self.stage = stage
self.name = componentDictReader.getDictName()
# Object is just a force, inertia is zero
self.inertia = Inertia(Vector(0, 0, 0), Vector(0, 0, 0), 0)
force = componentDictReader.getVector("force")
forceLocation = componentDictReader.getVector("position")
moment = componentDictReader.getVector("moment")
self.force = ForceMomentSystem(force, forceLocation, moment)
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 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))
class FractionalJetDamping(RocketComponent):
''' A component to model Jet damping as per NASA's Two Stage to Orbit verification case '''
# Object is just a force, inertia is zero
inertia = Inertia(Vector(0, 0, 0), Vector(0, 0, 0), 0)
def __init__(self, componentDictReader, rocket, stage):
self.rocket = rocket
self.stage = stage
self.componentDictReader = componentDictReader
self.name = componentDictReader.getDictName()
self.dampingFraction = componentDictReader.getFloat("fraction")
def getAppliedForce(self, rocketState, time, environmentalConditions,
rocketCG):
# Only apply damping force if current stage's engine is firing
# (Other stage's motors will have different exit planes)
if time > self.stage.motor.ignitionTime and time < self.stage.engineShutOffTime:
currentRocketInertia = self.rocket.getInertia(time, rocketState)
# Differentiate rate of MOI change
dt = 0.001
nextRocketInertia = self.rocket.getInertia(time + dt, rocketState)
MOIChangeRate = (currentRocketInertia.MOI.X -
nextRocketInertia.MOI.X) / dt
dampingFactor = MOIChangeRate * self.dampingFraction
angVel = rocketState.angularVelocity
dampingMoment = Vector(-angVel.X * dampingFactor,
-angVel.Y * dampingFactor, 0)
return ForceMomentSystem(Vector(0, 0, 0), moment=dampingMoment)
else:
return ForceMomentSystem(Vector(0, 0, 0))
def getInertia(self, time, state):
return self.inertia
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_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 getInertia(self, time, state):
inertiaData = linInterp(self.times, self.inertiaData, time)
# MOI is last three columns, CG is the three before that, and mass is column 0
return Inertia(Vector(*inertiaData[-3:]), Vector(*inertiaData[1:4]),
inertiaData[0])