# 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.importinspectimportmathimportosimportmatplotlib.pyplotaspltimportnumpyimportpytest## orb_elem_convert Unit Test## Purpose: Test the precision of the orb_elem_convert module. Functionality# is tested by comparing input/output data as well as calculated# conversions.# Author: Gabriel Chapel# Creation Date: July 27, 2017#filename=inspect.getframeinfo(inspect.currentframe()).filenamepath=os.path.dirname(os.path.abspath(filename))splitPath=path.split('simulation')fromBasilisk.utilitiesimportSimulationBaseClassfromBasilisk.simulationimportorbElemConvertfromBasilisk.utilitiesimportmacrosfromBasilisk.utilitiesimportmacrosasmcfromBasilisk.utilitiesimportunitTestSupportfromBasilisk.architectureimportmessagingfromBasilisk.utilities.pythonVariableLoggerimportPythonVariableLogger# Class in order to plot using data across the different parameterized scenariosclassDataStore:def__init__(self):self.Date=[]# replace these with appropriate containers for the data to be stored for plottingself.MarsPosErr=[]self.EarthPosErr=[]self.SunPosErr=[]
[docs]@pytest.mark.parametrize("a, e, i, AN, AP, f, mu, name",[# Inclined Elliptical Orbit Varying e(10000000.0,0.01,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'IncEllip_e_1'),(10000000.0,0.10,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(10000000.0,0.25,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(10000000.0,0.50,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(10000000.0,0.75,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'IncEllip_e_2'),# Inclined Elliptical Orbit Varying a(10000000.0,0.50,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'IncEllip_a_1'),(100000.0,0.50,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(10000.0,0.50,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(1000.0,0.50,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(100.0,0.50,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(10.0,0.50,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'IncEllip_a_2'),# Equatorial Elliptical Orbit Varying e(10000000.0,0.01,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'EquEllip_e_1'),(10000000.0,0.10,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(10000000.0,0.25,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(10000000.0,0.50,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(10000000.0,0.75,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'EquEllip_e_2'),# Equatorial Elliptical Orbit Varying a(10000000.0,0.50,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'EquEllip_a_1'),# For i=0 => AN=0(100000.0,0.50,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(10000.0,0.50,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(1000.0,0.50,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(100.0,0.50,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(10.0,0.50,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'EquEllip_a_2'),# Inclined Circular Orbit(10000000.0,0.0,33.3*mc.D2R,48.2*mc.D2R,0.0,85.3*mc.D2R,0.3986004415E+15,'IncCirc_1'),(1000000.0,0.0,33.3*mc.D2R,48.2*mc.D2R,0.0,85.3*mc.D2R,0.3986004415E+15,0),(100000.0,0.0,33.3*mc.D2R,48.2*mc.D2R,0.0,85.3*mc.D2R,0.3986004415E+15,0),(10000.0,0.0,33.3*mc.D2R,48.2*mc.D2R,0.0,85.3*mc.D2R,0.3986004415E+15,0),(1000.0,0.0,33.3*mc.D2R,48.2*mc.D2R,0.0,85.3*mc.D2R,0.3986004415E+15,0),(100.0,0.0,33.3*mc.D2R,48.2*mc.D2R,0.0,85.3*mc.D2R,0.3986004415E+15,0),(10.0,0.0,33.3*mc.D2R,48.2*mc.D2R,0.0,85.3*mc.D2R,0.3986004415E+15,'IncCirc_2'),# Equatorial Circular Orbit(10000000.0,0.0,0.0,0.0,0.0,85.3*mc.D2R,0.3986004415E+15,'EquCirc_1'),(1000000.0,0.0,0.0,0.0,0.0,85.3*mc.D2R,0.3986004415E+15,0),(100000.0,0.0,0.0,0.0,0.0,85.3*mc.D2R,0.3986004415E+15,0),(10000.0,0.0,0.0,0.0,0.0,85.3*mc.D2R,0.3986004415E+15,0),(1000.0,0.0,0.0,0.0,0.0,85.3*mc.D2R,0.3986004415E+15,0),(100.0,0.0,0.0,0.0,0.0,85.3*mc.D2R,0.3986004415E+15,0),(10.0,0.0,0.0,0.0,0.0,85.3*mc.D2R,0.3986004415E+15,'EquCirc_2'),# Inclined Parabolic Orbit(-10.0,1.0,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'IncPara_1'),# For input of -a,(-100.0,1.0,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),# must have e= 1.0(-1000.0,1.0,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),# or e >1.0(-10000.0,1.0,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(-100000.0,1.0,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'IncPara_2'),# Equatorial Parabolic Orbit(-10.0,1.0,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'EquPara_1'),# For input of -a,(-100.0,1.0,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),# must have e= 1.0(-1000.0,1.0,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),# or e >1.0(-10000.0,1.0,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(-100000.0,1.0,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'EquPara_2'),# Inclined Hyperbolic Orbit varying a(-10.0,1.3,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'IncHyp_a_1'),(-100.0,1.3,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(-1000.0,1.3,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(-10000.0,1.3,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(-100000.0,1.3,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'IncHyp_a_2'),# Inclined Hyperbolic Orbit varying e(-100000.0,1.1,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'IncHyp_e_1'),(-100000.0,1.2,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(-100000.0,1.3,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(-100000.0,1.4,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(-100000.0,1.5,33.3*mc.D2R,48.2*mc.D2R,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'IncHyp_e_2'),# Equatorial Hyperbolic Orbit varying a(-10.0,1.3,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'EquHyp_a_1'),(-100.0,1.3,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(-1000.0,1.3,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(-10000.0,1.3,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(-100000.0,1.3,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'EquHyp_a_2'),# Equatorial Hyperbolic Orbit varying e(-100000.0,1.1,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'EquHyp_e_1'),(-100000.0,1.2,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(-100000.0,1.3,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(-100000.0,1.4,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,0),(-100000.0,1.5,0.0,0.0,347.8*mc.D2R,85.3*mc.D2R,0.3986004415E+15,'EquHyp_e_2'),# # Approaching circular orbit# (100000.0, 0.000001, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15),# These don't work# (10000000.0, 1.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15), # or e >1.0# (-10, 0.9, 33.3*mc.D2R, 48.2*mc.D2R, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15)])# provide a unique test method name, starting with test_deftest_orb_elem_convert(a,e,i,AN,AP,f,mu,name,DispPlot=False):"""Module Unit Test"""# each test method requires a single assert method to be called[testResults,testMessage]=orbElem(a,e,i,AN,AP,f,mu,name,DispPlot)asserttestResults<1,testMessage
# Run unit testdeforbElem(a,e,i,AN,AP,f,mu,name,DispPlot):# Elem2RVtestFailCount1=0# zero unit test result countertestMessages=[]# create empty array to store test log messages# Create a sim module as an empty containerunitTaskName="unitTask"# arbitrary name (don't change)unitProcessName="TestProcess"# arbitrary name (don't change)# Create a sim module as an empty containerTotalSim=SimulationBaseClass.SimBaseClass()DynUnitTestProc=TotalSim.CreateNewProcess(unitProcessName)# # create the dynamics task and specify the integration update timetestProcessRate=macros.sec2nano(1.0)DynUnitTestProc.addTask(TotalSim.CreateNewTask(unitTaskName,testProcessRate))# Initialize the modules that we are using.orb_elemObject=orbElemConvert.OrbElemConvert()orb_elemObject.ModelTag="OrbElemConvertData"# Add Model To TaskTotalSim.AddModelToTask(unitTaskName,orb_elemObject)# Set element valuesepsDiff=1e-5orb_elemObject.mu=mu###### ELEM2RV ######orb_elemObjectLog=orb_elemObject.logger(["r_N","v_N"])TotalSim.AddModelToTask(unitTaskName,orb_elemObjectLog)# Create and write messagesElemMessage=messaging.ClassicElementsMsgPayload()elemMsg=messaging.ClassicElementsMsg()ife==1.0:ElemMessage.a=0.0ElemMessage.rPeriap=-aelse:ElemMessage.a=a# metersElemMessage.e=eElemMessage.i=iElemMessage.Omega=ANElemMessage.omega=APElemMessage.f=felemMsg.write(ElemMessage)orb_elemObject.elemInMsg.subscribeTo(elemMsg)# Log Message to test WriteOutputMessage()dataLog=orb_elemObject.spiceStateOutMsg.recorder()TotalSim.AddModelToTask(unitTaskName,dataLog)# Execute simulationTotalSim.ConfigureStopTime(int(1E9))TotalSim.InitializeSimulation()TotalSim.ExecuteSimulation()# Get r and v from simvSim=orb_elemObjectLog.v_N[-1]rSim=orb_elemObjectLog.r_N[-1]# Get r and v from messagerMsgPlanet=dataLog.PositionVector[-1]vMsgPlanet=dataLog.VelocityVector[-1]numpy.testing.assert_allclose(rSim,rMsgPlanet,rtol=epsDiff,verbose=True,err_msg="FAILED: Planet Position Message")numpy.testing.assert_allclose(vSim,vMsgPlanet,rtol=epsDiff,verbose=True,err_msg="FAILED: Planet Velocity Message")# Calculation of elem2rvife==1.0anda>0.0:# rectilinear elliptic orbit caseEcc=f# f is treated as ecc.anomalyr=a*(1-e*math.cos(Ecc))# orbit radiusv=math.sqrt(2*mu/r-mu/a)ir=numpy.zeros(3)ir[0]=math.cos(AN)*math.cos(AP)-math.sin(AN)*math.sin(AP)*math.cos(i)ir[1]=math.sin(AN)*math.cos(AP)+math.cos(AN)*math.sin(AP)*math.cos(i)ir[2]=math.sin(AP)*math.sin(i)rTruth=numpy.multiply(r,ir)ifmath.sin(Ecc)>0:vTruth=numpy.multiply(-v,ir)else:vTruth=numpy.multiply(v,ir)else:ife==1anda<0:# parabolic caserp=-a# radius at periapsesp=2*rp# semi-latus rectuma=0.0else:# elliptic and hyperbolic casesp=a*(1-e*e)# semi-latus rectumr=p/(1+e*math.cos(f))# orbit radiustheta=AP+f# true latitude angleh=math.sqrt(mu*p)# orbit ang.momentum mag.rTruth=numpy.zeros(3)rTruth[0]=r*(math.cos(AN)*math.cos(theta)-math.sin(AN)*math.sin(theta)*math.cos(i))rTruth[1]=r*(math.sin(AN)*math.cos(theta)+math.cos(AN)*math.sin(theta)*math.cos(i))rTruth[2]=r*(math.sin(theta)*math.sin(i))vTruth=numpy.zeros(3)vTruth[0]=-mu/h*(math.cos(AN)*(math.sin(theta)+e*math.sin(AP))+math.sin(AN)*(math.cos(theta)+e*math.cos(AP))*math.cos(i))vTruth[1]=-mu/h*(math.sin(AN)*(math.sin(theta)+e*math.sin(AP))-math.cos(AN)*(math.cos(theta)+e*math.cos(AP))*math.cos(i))vTruth[2]=-mu/h*(-(math.cos(theta)+e*math.cos(AP))*math.sin(i))# Position and Velocity Diff Checksnumpy.testing.assert_allclose(rSim,rTruth,rtol=epsDiff,verbose=True,err_msg="FAILED: Position Vector")numpy.testing.assert_allclose(vSim,vTruth,rtol=epsDiff,verbose=True,err_msg="FAILED: Velocity Vector")###### RV2ELEM ####### RV2ElemtestFailCount2=0# zero unit test result counterforginrange(2):TotalSim=SimulationBaseClass.SimBaseClass()DynUnitTestProc=TotalSim.CreateNewProcess(unitProcessName)# # create the dynamics task and specify the integration update timetestProcessRate=macros.sec2nano(1.0)DynUnitTestProc.addTask(TotalSim.CreateNewTask(unitTaskName,testProcessRate))# Initialize the modules that we are using.orb_elemObject=orbElemConvert.OrbElemConvert()orb_elemObject.ModelTag="OrbElemConvertData"# Add Model To TaskTotalSim.AddModelToTask(unitTaskName,orb_elemObject)# Log VariableselemLog=PythonVariableLogger({"a":lambda_:orb_elemObject.CurrentElem.a,"e":lambda_:orb_elemObject.CurrentElem.e,"i":lambda_:orb_elemObject.CurrentElem.i,"Omega":lambda_:orb_elemObject.CurrentElem.Omega,"omega":lambda_:orb_elemObject.CurrentElem.omega,"f":lambda_:orb_elemObject.CurrentElem.f,})TotalSim.AddModelToTask(unitTaskName,elemLog)orb_elemObjectLog=orb_elemObject.logger(["r_N","v_N"])TotalSim.AddModelToTask(unitTaskName,orb_elemObjectLog)orb_elemObject.mu=muifg==0:CartMessage=messaging.SCStatesMsgPayload()CartMessage.r_BN_N=rSimCartMessage.v_BN_N=vSimstateScMsg=messaging.SCStatesMsg().write(CartMessage)orb_elemObject.scStateInMsg.subscribeTo(stateScMsg)else:CartMessage=messaging.SpicePlanetStateMsgPayload()CartMessage.PositionVector=rSimCartMessage.VelocityVector=vSimstateSpMsg=messaging.SpicePlanetStateMsg().write(CartMessage)orb_elemObject.spiceStateInMsg.subscribeTo(stateSpMsg)dataElemLog=orb_elemObject.elemOutMsg.recorder()TotalSim.AddModelToTask(unitTaskName,dataElemLog)# Execute simulationTotalSim.ConfigureStopTime(int(1E9))TotalSim.InitializeSimulation()TotalSim.ExecuteSimulation()aOut=elemLog.a[-1]eOut=elemLog.e[-1]iOut=elemLog.i[-1]ANOut=elemLog.Omega[-1]APOut=elemLog.omega[-1]fOut=elemLog.f[-1]# Element Diff CheckElemDiff=[(a-aOut),(e-eOut),(i-iOut),(AN-ANOut),(AP-APOut),(f-fOut)]ElemDiffcsv=numpy.asarray(ElemDiff)forginrange(6):# check for angle roll over with 2*piifg>2:ifabs(ElemDiff[g]-2*math.pi)<epsDiff:ElemDiff[g]-=2*math.pielifabs(ElemDiff[g]+2*math.pi)<epsDiff:ElemDiff[g]+=2*math.piifabs(ElemDiff[g])>epsDiff:testMessages.append(" FAILED: Sim Orbital Element "+str(g))testFailCount2+=1aMsg=dataElemLog.a[-1]eMsg=dataElemLog.e[-1]iMsg=dataElemLog.i[-1]ANMsg=dataElemLog.Omega[-1]APMsg=dataElemLog.omega[-1]fMsg=dataElemLog.f[-1]ElemMsgDiff=[(aOut-aMsg),(eOut-eMsg),(iOut-iMsg),(ANOut-ANMsg),(APOut-APMsg),(fOut-fMsg)]forginrange(6):# check for angle roll over with 2*piifg>2:ifabs(ElemDiff[g]-2*math.pi)<epsDiff:ElemDiff[g]-=2*math.pielifabs(ElemDiff[g]+2*math.pi)<epsDiff:ElemDiff[g]+=2*math.piifabs(ElemMsgDiff[g])>0:testMessages.append(" FAILED: Orbital Element Message "+str(g))testFailCount2+=1######### Calculate rv2elem ########## Calculate the specific angular momentum and its magnitudeepsConv=0.0000000001hVec=numpy.cross(rTruth,vTruth)h=numpy.linalg.norm(hVec)p=h*h/mu# Calculate the line of nodesv3=numpy.array([0.0,0.0,1.0])nVec=numpy.cross(v3,hVec)n=numpy.linalg.norm(nVec)# Orbit eccentricity and energyr=numpy.linalg.norm(rTruth)v=numpy.linalg.norm(vTruth)eVec=numpy.multiply(v*v/mu-1.0/r,rTruth)v3=numpy.multiply(numpy.dot(rTruth,vTruth)/mu,vTruth)eVec=numpy.subtract(eVec,v3)eO=numpy.linalg.norm(eVec)# compute semi - major axisalpha=2.0/r-v*v/muif(math.fabs(alpha)>epsConv):# elliptic or hyperbolic caseaO=1.0/alphaelse:# parabolic caseaO=0.0# a is not defined for parabola, so -rp is returned instead# Calculate the inclinationiO=math.acos(hVec[2]/h)# Calculate AP, AN, and True anomalyifeO>=1e-11andiO>=1e-11:# Case 1: Non - circular, inclined orbitOmega=math.acos(nVec[0]/n)if(nVec[1]<0.0):Omega=2.0*math.pi-Omegaomega=math.acos(numpy.dot(nVec,eVec)/n/eO)ifeVec[2]<0.0:omega=2.0*math.pi-omegafO=math.acos(numpy.dot(eVec,rTruth)/eO/r)ifnumpy.dot(rTruth,vTruth)<0.0:fO=2.0*math.pi-fOelifeO>=1e-11andiO<1e-11:# Case 2: Non - circular, equatorial orbit# Equatorial orbit has no ascending nodeOmega=0.0# True longitude of periapsis, omegatilde_trueomega=math.acos(eVec[0]/eO)ifeVec[1]<0.0:omega=2.0*math.pi-omegafO=math.acos(numpy.dot(eVec,rTruth)/eO/r)ifnumpy.dot(rTruth,vTruth)<0.0:fO=2.0*math.pi-fOelifeO<1e-11andiO>=1e-11:# Case 3: Circular, inclined orbitOmega=math.acos(nVec[0]/n)if(nVec[1]<0.0):Omega=2.0*math.pi-Omegaomega=0.0# Argument of latitude, u = omega + f * /fO=math.acos(numpy.dot(nVec,rTruth)/n/r)ifrTruth[2]<0.0:fO=2.0*math.pi-fOelifeO<1e-11andiO<1e-11:# Case 4: Circular, equatorial orbitOmega=0.0omega=0.0# True longitude, lambda_truefO=math.acos(rTruth[0]/r)ifrTruth[1]<0:fO=2.0*math.pi-fOelse:print("Error: rv2elem couldn't identify orbit type.\n")if(eO>=1.0andmath.fabs(fO)>math.pi):twopiSigned=math.copysign(2.0*math.pi,fO)fO-=twopiSigned# Element Diff CheckElemCalcDiff=[(aO-aOut),(eO-eOut),(iO-iOut),(Omega-ANOut),(omega-APOut),(fOut-fOut)]ElemCalcDiffcsv=numpy.asarray(ElemCalcDiff)forginrange(6):# check for angle roll over with 2*piifg>2:ifabs(ElemCalcDiff[g]-2*math.pi)<epsDiff:ElemCalcDiff[g]-=2*math.pielifabs(ElemCalcDiff[g]+2*math.pi)<epsDiff:ElemCalcDiff[g]+=2*math.piifabs(ElemCalcDiff[g])>epsDiff:testMessages.append(" FAILED: Calculated Orbital Element "+str()+str(g))testFailCount2+=1# create plot# txt = 'e = ' + str(e) + ' and a = ' + str(a) + 'km'fact=(len(str(abs(a)))-3.0)plt.close('all')plt.figure(1,figsize=(7,5),dpi=80,facecolor='w',edgecolor='k')# fig1.text(.5, .05, txt, ha='center')ax1=plt.subplot(211)ax1.cla()index=numpy.arange(3)bar_width=0.35opacity=0.8rects1=ax1.bar(index,rSim,bar_width,alpha=opacity,color='b',label='Simulated Position')rects2=ax1.bar(index+bar_width,rTruth,bar_width,alpha=opacity,color='g',label='Calculated Position')ax1.spines['left'].set_position('zero')ax1.spines['right'].set_color('none')ax1.spines['bottom'].set_position('zero')ax1.spines['top'].set_color('none')forxtickinax1.get_xticklabels():xtick.set_bbox(dict(facecolor='white',edgecolor='None',alpha=0.5))ax1.xaxis.set_ticks_position('bottom')ax1.yaxis.set_ticks_position('left')plt.ylabel('Position (m)')plt.xticks(index+bar_width,('X','Y','Z'))plt.legend(loc='lower right')ax2=plt.subplot(212)ax2.cla()rects1=ax2.bar(index,vSim,bar_width,alpha=opacity,color='b',label='Simulated Velocity')rects2=ax2.bar(index+bar_width,vTruth,bar_width,alpha=opacity,color='g',label='Calculated Velocity')ax2.spines['left'].set_position('zero')ax2.spines['right'].set_color('none')ax2.spines['bottom'].set_position('zero')ax2.spines['top'].set_color('none')forxtickinax2.get_xticklabels():xtick.set_bbox(dict(facecolor='white',edgecolor='None',alpha=0.5))ax2.xaxis.set_ticks_position('bottom')ax2.yaxis.set_ticks_position('left')plt.ylabel('Velocity (m/s)')plt.xticks(index+bar_width,('X','Y','Z'))plt.legend(loc='lower right')ifname!=0:unitTestSupport.writeFigureLaTeX(name,"$e = "+str(e)+"$ and $a = 10^"+str(int(fact))+"$km",plt,'height=0.7\\textwidth, keepaspectratio',path)iftestFailCount2==0:colorText='ForestGreen'passFailMsg=""# "Passed: " + name + "."passedText=r'\textcolor{'+colorText+'}{'+"PASSED"+'}'else:colorText='Red'passFailMsg="Failed: "+name+"."testMessages.append(passFailMsg)testMessages.append(" | ")passedText=r'\textcolor{'+colorText+'}{'+"FAILED"+'}'# Write some snippets for AutoTexsnippetName=name+"PassedText2"snippetContent=passedTextunitTestSupport.writeTeXSnippet(snippetName,snippetContent,path)snippetName=name+"PassFailMsg2"snippetContent=passFailMsgunitTestSupport.writeTeXSnippet(snippetName,snippetContent,path)ifDispPlot:plt.show()plt.close()testFailCount=testFailCount1+testFailCount2iftestFailCount:print("Failed")print(testMessages)else:print("PASSED")return[testFailCount,''.join(testMessages)]if__name__=="__main__":orbElem(10000000.0,0.0,33.3*mc.D2R,48.2*mc.D2R,0.0,85.3*mc.D2R,0.3986004415E+15,0,True)