示例#1
0
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)
示例#2
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())
示例#3
0
    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))
示例#4
0
    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))
示例#5
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)
示例#6
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
示例#7
0
 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
示例#8
0
    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)
示例#9
0
    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))
示例#10
0
    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)
示例#12
0
    def getConvertedQuaternion(self, currentRigidBodyState):

        conversionQuaternion = Quaternion(axisOfRotation=Vector(1, 0, 0),
                                          angle=3.141592654)
        convertedQuaternion = currentRigidBodyState.orientation * conversionQuaternion

        return currentRigidBodyState.orientation
示例#13
0
    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))
示例#14
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)
示例#15
0
 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))
示例#16
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)
示例#17
0
    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
示例#18
0
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)
示例#19
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
        )
示例#20
0
    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)))
示例#21
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
示例#22
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)
示例#23
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))
示例#24
0
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
示例#25
0
    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
示例#26
0
    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))
示例#27
0
    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))
示例#28
0
    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])
示例#30
0
    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)