Source code for test_chebyPosEphem


# ISC License
#
# Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
#
# Permission to use, copy, modify, and/or distribute this software for any
# purpose with or without fee is hereby granted, provided that the above
# copyright notice and this permission notice appear in all copies.
#
# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.


import inspect
import math
import os

import numpy
import pytest

filename = inspect.getframeinfo(inspect.currentframe()).filename
path = os.path.dirname(os.path.abspath(filename))
from Basilisk import __path__
bskPath = __path__[0]

from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import macros
from Basilisk.fswAlgorithms import chebyPosEphem
from Basilisk.topLevelModules import pyswice
from Basilisk.utilities.pyswice_spk_utilities import spkRead
import matplotlib.pyplot as plt
from Basilisk.architecture import messaging

orbitPosAccuracy = 1.0
orbitVelAccuracy = 0.01

# uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed
# @pytest.mark.skipif(conditionstring)
# uncomment this line if this test has an expected failure, adjust message as needed
# @pytest.mark.xfail() # need to update how the RW states are defined
# provide a unique test method name, starting with test_
[docs] @pytest.mark.parametrize("function", ["sineCosine" , "earthOrbitFit" ]) def test_chebyPosFitAllTest(show_plots, function): """Module Unit Test""" [testResults, testMessage] = eval(function + '(show_plots)') assert testResults < 1, testMessage
[docs] def sineCosine(show_plots): """Module Unit Test""" # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. __tracebackhide__ = True testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages orbitRadius = 70000.0 numCurvePoints = 365*3+1 curveDurationDays = 365.0*3 degChebCoeff =21 angleSpace = numpy.linspace(-3*math.pi, 3*math.pi, numCurvePoints) cosineValues = numpy.cos(angleSpace)*orbitRadius sineValues = numpy.sin(angleSpace)*orbitRadius oopValues = numpy.sin(angleSpace) + orbitRadius pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/naif0012.tls') et = pyswice.new_doubleArray(1) timeStringMid = '2019 APR 1 12:12:12.0 (UTC)' pyswice.str2et_c(timeStringMid, et) fitTimes = numpy.linspace(-1, 1, numCurvePoints) chebCosCoeff = numpy.polynomial.chebyshev.chebfit(fitTimes, cosineValues, degChebCoeff) chebSinCoeff = numpy.polynomial.chebyshev.chebfit(fitTimes, sineValues, degChebCoeff) cheboopCoeff = numpy.polynomial.chebyshev.chebfit(fitTimes, oopValues, degChebCoeff) unitTaskName = "unitTask" # arbitrary name (don't change) unitProcessName = "TestProcess" # arbitrary name (don't change) # Create a sim module as an empty container TotalSim = SimulationBaseClass.SimBaseClass() FSWUnitTestProc = TotalSim.CreateNewProcess(unitProcessName) # create the dynamics task and specify the integration update time FSWUnitTestProc.addTask(TotalSim.CreateNewTask(unitTaskName, macros.sec2nano(8640.0))) chebyFitModel = chebyPosEphem.chebyPosEphem() chebyFitModel.ModelTag = "chebyFitModel" TotalSim.AddModelToTask(unitTaskName, chebyFitModel) totalList = numpy.array(chebCosCoeff).tolist() totalList.extend(numpy.array(chebSinCoeff).tolist()) totalList.extend(numpy.array(cheboopCoeff).tolist()) chebyFitModel.ephArray[0].posChebyCoeff = totalList chebyFitModel.ephArray[0].nChebCoeff = degChebCoeff+1 chebyFitModel.ephArray[0].ephemTimeMid = pyswice.doubleArray_getitem(et, 0) chebyFitModel.ephArray[0].ephemTimeRad = curveDurationDays/2.0*86400.0 clockCorrData = messaging.TDBVehicleClockCorrelationMsgPayload() clockCorrData.vehicleClockTime = 0.0 clockCorrData.ephemerisTime = chebyFitModel.ephArray[0].ephemTimeMid - \ chebyFitModel.ephArray[0].ephemTimeRad clockInMsg = messaging.TDBVehicleClockCorrelationMsg().write(clockCorrData) chebyFitModel.clockCorrInMsg.subscribeTo(clockInMsg) xFitData = numpy.polynomial.chebyshev.chebval(fitTimes, chebCosCoeff) dataLog = chebyFitModel.posFitOutMsg.recorder() TotalSim.AddModelToTask(unitTaskName, dataLog) TotalSim.InitializeSimulation() TotalSim.ConfigureStopTime(int(curveDurationDays*86400.0*1.0E9)) TotalSim.ExecuteSimulation() posChebData = dataLog.r_BdyZero_N angleSpaceFine = numpy.linspace(-3*math.pi, 3*math.pi, numCurvePoints*10-9) cosineValuesFine = numpy.cos(angleSpaceFine)*orbitRadius sineValuesFine = numpy.sin(angleSpaceFine)*orbitRadius oopValuesFine = numpy.sin(angleSpaceFine) + orbitRadius maxErrVec = [max(abs(posChebData[:,0] - cosineValuesFine)), max(abs(posChebData[:,1] - sineValuesFine)), max(abs(posChebData[:,2] - oopValuesFine))] print("Sine Wave error: " + str(max(maxErrVec))) assert max(maxErrVec) < orbitPosAccuracy if testFailCount == 0: print("PASSED: " + " Sine and Cosine curve fit") # return fail count and join into a single string all messages in the list # testMessage return [testFailCount, ''.join(testMessages)]
def earthOrbitFit(show_plots): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. #__tracebackhide__ = True testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages numCurvePoints = 365*3+1 curveDurationSeconds = 3*5950.0 degChebCoeff =23 integFrame = "j2000" zeroBase = "Earth" dateSpice = "2015 February 10, 00:00:00.0 TDB" pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/naif0012.tls') et = pyswice.new_doubleArray(1) pyswice.str2et_c(dateSpice, et) etStart = pyswice.doubleArray_getitem(et, 0) etEnd = etStart + curveDurationSeconds pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/de430.bsp') pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/naif0012.tls') pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/de-403-masses.tpc') pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/pck00010.tpc') pyswice.furnsh_c(path + '/hst_edited.bsp') hubblePosList = [] hubbleVelList = [] timeHistory = numpy.linspace(etStart, etEnd, numCurvePoints) for timeVal in timeHistory: stringCurrent = pyswice.et2utc_c(timeVal, 'C', 4, 1024, "Yo") stateOut = spkRead('HUBBLE SPACE TELESCOPE', stringCurrent, integFrame, zeroBase) hubblePosList.append(stateOut[0:3].tolist()) hubbleVelList.append(stateOut[3:6].tolist()) hubblePosList = numpy.array(hubblePosList) hubbleVelList = numpy.array(hubbleVelList) fitTimes = numpy.linspace(-1, 1, numCurvePoints) chebCoeff = numpy.polynomial.chebyshev.chebfit(fitTimes, hubblePosList, degChebCoeff) unitTaskName = "unitTask" # arbitrary name (don't change) unitProcessName = "TestProcess" # arbitrary name (don't change) # Create a sim module as an empty container TotalSim = SimulationBaseClass.SimBaseClass() FSWUnitTestProc = TotalSim.CreateNewProcess(unitProcessName) # create the dynamics task and specify the integration update time FSWUnitTestProc.addTask(TotalSim.CreateNewTask(unitTaskName, macros.sec2nano(curveDurationSeconds/(numCurvePoints-1)))) chebyFitModel = chebyPosEphem.chebyPosEphem() chebyFitModel.ModelTag = "chebyFitModel" TotalSim.AddModelToTask(unitTaskName, chebyFitModel) totalList = chebCoeff[:,0].tolist() totalList.extend(chebCoeff[:,1].tolist()) totalList.extend(chebCoeff[:,2].tolist()) chebyFitModel.ephArray[0].posChebyCoeff = totalList chebyFitModel.ephArray[0].nChebCoeff = degChebCoeff+1 chebyFitModel.ephArray[0].ephemTimeMid = etStart + curveDurationSeconds/2.0 chebyFitModel.ephArray[0].ephemTimeRad = curveDurationSeconds/2.0 clockCorrData = messaging.TDBVehicleClockCorrelationMsgPayload() clockCorrData.vehicleClockTime = 0.0 clockCorrData.ephemerisTime = chebyFitModel.ephArray[0].ephemTimeMid - \ chebyFitModel.ephArray[0].ephemTimeRad clockInMsg = messaging.TDBVehicleClockCorrelationMsg().write(clockCorrData) chebyFitModel.clockCorrInMsg.subscribeTo(clockInMsg) dataLog = chebyFitModel.posFitOutMsg.recorder() TotalSim.AddModelToTask(unitTaskName, dataLog) TotalSim.InitializeSimulation() TotalSim.ConfigureStopTime(int(curveDurationSeconds*1.0E9)) TotalSim.ExecuteSimulation() posChebData = dataLog.r_BdyZero_N velChebData = dataLog.v_BdyZero_N maxErrVec = [abs(max(posChebData[:,0] - hubblePosList[:,0])), abs(max(posChebData[:,1] - hubblePosList[:,1])), abs(max(posChebData[:,2] - hubblePosList[:,2]))] maxVelErrVec = [abs(max(velChebData[:,0] - hubbleVelList[:,0])), abs(max(velChebData[:,1] - hubbleVelList[:,1])), abs(max(velChebData[:,2] - hubbleVelList[:,2]))] print("Hubble Orbit Accuracy: " + str(max(maxErrVec))) print("Hubble Velocity Accuracy: " + str(max(maxVelErrVec))) assert (max(maxErrVec)) < orbitPosAccuracy assert (max(maxVelErrVec)) < orbitVelAccuracy plt.figure() plt.plot(dataLog.times()*1.0E-9, velChebData[:,0], dataLog.times()*1.0E-9, hubbleVelList[:,0]) if(show_plots): plt.show() plt.close('all') if testFailCount == 0: print("PASSED: " + " Orbit curve fit") # return fail count and join into a single string all messages in the list # testMessage return [testFailCount, ''.join(testMessages)] if __name__ == "__main__": test_chebyPosFitAllTest(True)