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)