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