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)
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 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_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_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_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
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 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 getConvertedQuaternion(self, currentRigidBodyState):
conversionQuaternion = Quaternion(axisOfRotation=Vector(1, 0, 0),
angle=3.141592654)
convertedQuaternion = currentRigidBodyState.orientation * conversionQuaternion
return currentRigidBodyState.orientation
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_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_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 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 getInertialToENUFrameRotation(self, x, y, z):
''' Returns a Quaternion that defines the rotation b/w the global inertia frame and the local, surface-normal North-East-Up (y-x-z) frame '''
# Time offset not necessary, since the x, y, z coordinates are inertial, and we are not
# interested in finding a particular location on the surface of the earth,
# we are just interested in finding the surface normal under a x/y/z location.
# The result is independent of the earth's rotation.
lat, lon, height = self.cartesianToGeodetic(x, y, z)
# Rotation between inertial frame and ENU frame will be composed in two steps
# Step 1: Rotate about inertial/initial Z-axis to Y-axis in such a way, that after rotation 2,
# it will be pointing North
rot1Angle = radians(lon + 90)
rot1 = Quaternion(axisOfRotation=Vector(0, 0, 1), angle=rot1Angle)
# Step 2: Rotate about the local x-axis to match the latitude
rot2Angle = radians(90 - lat)
rot2 = Quaternion(axisOfRotation=Vector(1, 0, 0), angle=rot2Angle)
# Sequentially combine rotations by multiplying them
return rot1 * rot2
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 _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):
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 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_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 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 _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
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 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))
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 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 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)