## ISC License## Copyright (c) 2022, 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.#r"""Overview--------This simulation demonstrates how to put a spacecraft in orbit about a custom gravitational body while conducting severalattitude changes. Several attitude pointing modes are implemented, along with other visual tools including antennatransmission and thruster visualization.The spacecraft starts on a elliptical orbit towards the asteroid Bennu. The spacecraft conducts aburn at periapsis of the elliptical orbit, transferring to a circular orbit about Bennu with a radius of 800meters. The spacecraft then completes a series of Hohmann transfers while also conducting several attitude changesuntil reaching a final elliptical orbit about the asteroid.The script is found in the folder ``basilisk/examples`` and executed by using:: python3 scenarioAsteroidArrival.py.. attention:: To see the asteroid Bennu in Vizard the asteroid asset bundle must be installed. See the Vizard `Download <http://hanspeterschaub.info/basilisk/Vizard/VizardDownload.html>`__ web page.Setting Up The Custom Gravitational Body----------------------------------------Because Spice will not be used to generate the ephemeris information for Bennu, an instance of the module``planetEphemeris`` is created to generate Bennu's ephemeris:: gravBodyEphem = planetEphemeris.PlanetEphemeris() gravBodyEphem.ModelTag = 'planetEphemeris' scSim.AddModelToTask(simTaskName, gravBodyEphem) gravBodyEphem.setPlanetNames(planetEphemeris.StringVector(["Bennu"]))Next, the module is configured by specifying the orbital parameters of Bennu:: timeInitString = "2011 January 1 0:00:00.0" diam = 2 * 245.03 # m G = 6.67408 * (10 ** -11) # m^3 / kg*s^2 massBennu = 7.329 * (10 ** 10) # kg mu = G * massBennu # Bennu grav. parameter, m^3/s^2 oeAsteroid = planetEphemeris.ClassicElements() oeAsteroid.a = 1.1264 * astroConstants.AU * 1000 # m oeAsteroid.e = 0.20375 oeAsteroid.i = 6.0349 * macros.D2R oeAsteroid.Omega = 2.0609 * macros.D2R oeAsteroid.omega = 66.2231 * macros.D2R oeAsteroid.f = 0.0 * macros.D2R gravBodyEphem.planetElements = planetEphemeris.classicElementVector([oeAsteroid]) gravBodyEphem.rightAscension = planetEphemeris.DoubleVector([85.65 * macros.D2R]) gravBodyEphem.declination = planetEphemeris.DoubleVector([-60.17 * macros.D2R]) gravBodyEphem.lst0 = planetEphemeris.DoubleVector([0.0 * macros.D2R]) gravBodyEphem.rotRate = planetEphemeris.DoubleVector([360 * macros.D2R / (4.296057 * 3600.)]) # rad/secNext, Bennu can be created as a gravitational body using the ``createCustomGravObject()`` method:: asteroid = gravFactory.createCustomGravObject("Bennu", mu) asteroid.isCentralBody = True # ensure this is the central gravitational bodyFinally, subscribe the custom gravitational body ``planetBodyInMsg`` to the planetEphemeris output message``planetOutMsgs``:: asteroid.planetBodyInMsg.subscribeTo(gravBodyEphem.planetOutMsgs[0])The spacecraft object is then created and all gravitational bodies are connected to the spacecraft.Recall that when configuring the ephemeris converter module, Bennu was not created with Spice. Therefore its inputmessage is of type ``planetEphemeris``:: ephemObject.addSpiceInputMsg(gravBodyEphem.planetOutMsgs[0])Implementing Attitude Pointing Modes------------------------------------After the spacecraft's initial orbital elements about Bennu are set using the ``orbitalMotion`` module, the attitudemodules and modes are created and configured. The four attitude pointing modes incorporated into this script includeEarth-pointing using the spacecraft's antenna with transmission visualization, Sun-pointing with the spacecraft'ssolar panel normal axis, orbital velocity pointing while conducting thruster burn visualizations, and science-pointingtowards the asteroid using a sensor created on the spacecraft... important:: Refer to the integrated example script :ref:`scenarioFlybySpice` for a more detailed discussion on configuring attitude modules and modes for a mission scenario.To execute the desired attitude-pointing mode, the run flight mode function must be calledwith the desired simulation time:: runAntennaEarthPointing(desiredSimTimeSec)Additional Visualization Features---------------------------------To add a visualization of antenna transmission back to Earth during the Earth-pointing mode wecan't use the typical way of adding these generic sensors, thrusters, etc. The reason is that we want to illustrate athruster, but we are not using a thruster effector. Thus, to add a thruster to the Vizard binarywe need to manually add these to the ``vizInterface`` spacecraft data structure.First, as is typical, a transceiver is created through the ``vizInterface``:: transceiverHUD = vizInterface.Transceiver() transceiverHUD.r_SB_B = [0., 0., 1.38] transceiverHUD.fieldOfView = 40.0 * macros.D2R transceiverHUD.normalVector = [0., 0., 1.] transceiverHUD.color = vizInterface.IntVector(vizSupport.toRGBA255("cyan")) transceiverHUD.label = "antenna" transceiverHUD.animationSpeed = 1To add a sensor visualization for the science-pointing mode, a sensor is created using the ``vizInterface``:: genericSensor = vizInterface.GenericSensor() genericSensor.r_SB_B = cameraLocation genericSensor.fieldOfView.push_back(10.0 * macros.D2R) genericSensor.fieldOfView.push_back(10.0 * macros.D2R) genericSensor.normalVector = cameraLocation genericSensor.size = 10 genericSensor.color = vizInterface.IntVector(vizSupport.toRGBA255("white", alpha=0.1)) genericSensor.label = "scienceCamera" genericSensor.genericSensorCmd = 1To add a camera to the science-pointing mode, the ``createStandardCamera`` method is used:: vizSupport.createStandardCamera(viz, setMode=1, spacecraftName=scObject.ModelTag, fieldOfView=10 * macros.D2R, pointingVector_B=[0,1,0], position_B=cameraLocation)Finally, to add a thruster visualization for the thruster burn mode, the ``vizInterface`` is again invoked.Here we manually add the Vizard interface elements back in to redo what the ``enableUnityVisualization()``normally does for us. The main difference is that we are manually setting the thruster information asthe spacecraft dynamics does not contain a thruster effector:: scData = vizInterface.VizSpacecraftData() scData.spacecraftName = scObject.ModelTag scData.scStateInMsg.subscribeTo(scObject.scStateOutMsg) scData.transceiverList = vizInterface.TransceiverVector([transceiverHUD]) scData.genericSensorList = vizInterface.GenericSensorVector([genericSensor]) thrusterMsgInfo = messaging.THROutputMsgPayload() thrusterMsgInfo.maxThrust = 1 # Newtons thrusterMsgInfo.thrustForce = 0 # Newtons thrusterMsgInfo.thrusterLocation = [0, 0, -1.5] thrusterMsgInfo.thrusterDirection = [0, 0, 1] thrMsg = messaging.THROutputMsg().write(thrusterMsgInfo) scData.thrInMsgs = messaging.THROutputInMsgsVector([thrMsg.addSubscriber()])After running the ``enableUnityVisualization()`` method, we need to clear the ``vizInterface`` spacecraftdata container ``scData`` and push our custom copy to it:: viz.scData.clear() viz.scData.push_back(scData)Illustration of Simulation Results----------------------------------The following image illustrates the expected simulation run return for the case when plots are requested... image:: /_images/Scenarios/scenarioAsteroidArrival1.svg :align: centerVisualization In Vizard----------------------------------The following image illustrates the expected visualization of this simulation script... image:: /_images/static/scenarioAsteroidArrival2.jpg :align: center"""## Basilisk Scenario Script and Integrated Test## Purpose: Basic simulation showing how to setup a flyby capture orbit about a custom gravity body.# Author: Leah Kiner# Creation Date: Feb. 6, 2022#importosimportmatplotlib.pyplotaspltimportnumpyasnpfromBasiliskimport__path__bskPath=__path__[0]fileName=os.path.basename(os.path.splitext(__file__)[0])fromBasilisk.utilitiesimport(SimulationBaseClass,macros,simIncludeGravBody,vizSupport,unitTestSupport,orbitalMotion)fromBasilisk.simulationimportspacecraft,extForceTorque,simpleNav,ephemerisConverter,planetEphemerisfromBasilisk.fswAlgorithmsimportmrpFeedback,attTrackingError,velocityPoint,locationPointingfromBasilisk.architectureimportmessaging,astroConstantstry:fromBasilisk.simulationimportvizInterfaceexceptImportError:pass# The path to the location of Basilisk# Used to get the location of supporting data.fileName=os.path.basename(os.path.splitext(__file__)[0])
[docs]defrun(show_plots):""" The scenarios can be run with the followings setups parameters: Args: show_plots (bool): Determines if the script should display plots """# Create simulation variable namessimTaskName="simTask"simProcessName="simProcess"# Configure the simulationscSim=SimulationBaseClass.SimBaseClass()# Shows the simulation progress bar in the terminalscSim.SetProgressBar(True)# Create the simulation processdynProcess=scSim.CreateNewProcess(simProcessName)# Create the dynamics task and specify the simulation time step informationsimulationTimeStep=macros.sec2nano(20.0)# Add dynamics task to the simulation processdynProcess.addTask(scSim.CreateNewTask(simTaskName,simulationTimeStep))# Setup celestial object ephemeris module for the asteroidgravBodyEphem=planetEphemeris.PlanetEphemeris()gravBodyEphem.ModelTag='planetEphemeris'scSim.AddModelToTask(simTaskName,gravBodyEphem)gravBodyEphem.setPlanetNames(planetEphemeris.StringVector(["bennu"]))# Specify orbital parameters of the asteroidtimeInitString="2011 January 1 0:00:00.0"diam=2*245.03# mG=6.67408*(10**-11)# m^3 / kg*s^2massBennu=7.329*(10**10)# kgmu=G*massBennu# Bennu grav. parameter, m^3/s^2oeAsteroid=planetEphemeris.ClassicElements()oeAsteroid.a=1.1264*astroConstants.AU*1000# moeAsteroid.e=0.20375oeAsteroid.i=6.0349*macros.D2RoeAsteroid.Omega=2.0609*macros.D2RoeAsteroid.omega=66.2231*macros.D2RoeAsteroid.f=0.0*macros.D2RgravBodyEphem.planetElements=planetEphemeris.classicElementVector([oeAsteroid])# Specify orientation parameters of the asteroidgravBodyEphem.rightAscension=planetEphemeris.DoubleVector([85.65*macros.D2R])gravBodyEphem.declination=planetEphemeris.DoubleVector([-60.17*macros.D2R])gravBodyEphem.lst0=planetEphemeris.DoubleVector([0.0*macros.D2R])gravBodyEphem.rotRate=planetEphemeris.DoubleVector([360*macros.D2R/(4.296057*3600.)])# rad/sec# Set orbital radii about asteroidr0=diam/2.0+800# capture orbit, metersr1=diam/2.0+600# intermediate orbit, metersr2=diam/2.0+400# final science orbit, metersr3=diam/2.0+200# meters, very close fly-by, elliptic orbitrP=r0rA=3*rP# Set orbital periodsP0=np.pi*2/np.sqrt(mu/(r0**3))P01=np.pi*2/np.sqrt(mu/(((r0+r1)/2)**3))P1=np.pi*2/np.sqrt(mu/(r1**3))P12=np.pi*2/np.sqrt(mu/(((r1+r2)/2)**3))P2=np.pi*2/np.sqrt(mu/(r2**3))P23=np.pi*2/np.sqrt(mu/(((r2+r3)/2)**3))# Create additional gravitational bodiesgravFactory=simIncludeGravBody.gravBodyFactory()gravFactory.createBodies("earth","sun")# Set gravity body index valuesearthIdx=0sunIdx=1asteroidIdx=2# Create and configure the default SPICE support module. The first step is to store# the date and time of the start of the simulation.spiceObject=gravFactory.createSpiceInterface(time=timeInitString,epochInMsg=True)# Add the SPICE object to the simulation task listscSim.AddModelToTask(simTaskName,spiceObject)# Create the asteroid custom gravitational bodyasteroid=gravFactory.createCustomGravObject("bennu",mu,modelDictionaryKey="Bennu",radEquator=565./2.0)asteroid.isCentralBody=True# ensures the asteroid is the central gravitational bodyasteroid.planetBodyInMsg.subscribeTo(gravBodyEphem.planetOutMsgs[0])# connect asteroid ephem. to custom grav body# Create the spacecraft objectscObject=spacecraft.Spacecraft()scObject.ModelTag="bskSat"# Connect all gravitational bodies to the spacecraftgravFactory.addBodiesTo(scObject)scSim.AddModelToTask(simTaskName,scObject)# Create an ephemeris converter to convert messages of type# 'SpicePlanetStateMsgPayload' to 'EphemerisMsgPayload'ephemObject=ephemerisConverter.EphemerisConverter()ephemObject.ModelTag='EphemData'ephemObject.addSpiceInputMsg(spiceObject.planetStateOutMsgs[earthIdx])ephemObject.addSpiceInputMsg(spiceObject.planetStateOutMsgs[sunIdx])# Recall the asteroid was not created with Spice.ephemObject.addSpiceInputMsg(gravBodyEphem.planetOutMsgs[0])scSim.AddModelToTask(simTaskName,ephemObject)# Define the spacecraft inertiaI=[900.,0.,0.,0.,800.,0.,0.,0.,600.]scObject.hub.mHub=750.0# kg - spacecraft massscObject.hub.r_BcB_B=[[0.0],[0.0],[0.0]]# m - position vector of body-fixed point B relative to CMscObject.hub.IHubPntBc_B=unitTestSupport.np2EigenMatrix3d(I)# Define the initial spacecraft orbit about the asteroidoe=orbitalMotion.ClassicElements()oe.a=(rP+rA)/2.0oe.e=1-(rP/oe.a)oe.i=90.0*macros.D2Roe.Omega=180.0*macros.D2Roe.omega=347.8*macros.D2Roe.f=-45.0*macros.D2REcc=np.arctan(np.tan(-oe.f/2)*np.sqrt((1-oe.e)/(1+oe.e)))*2# eccentric anomalyM=Ecc-oe.e*np.sin(Ecc)# mean anomalyn=np.sqrt(mu/(oe.a**3))h=np.sqrt(mu*oe.a*(1-oe.e**2))# specific angular momentumvP=h/rPV_SC_C_B=np.sqrt(mu/rP)# [m/s] (2) spacecraft circular parking speed relative to bennu.Delta_V_Parking_Orbit=V_SC_C_B-vP# Setting initial position and velocity vectors using orbital elementsr_N,v_N=orbitalMotion.elem2rv(mu,oe)T1=M/n# time until spacecraft reaches periapsis of arrival trajectory# Initialize spacecraft states with the initialization variablesscObject.hub.r_CN_NInit=r_N# [m] = r_BN_NscObject.hub.v_CN_NInit=v_N# [m/s] = v_BN_NscObject.hub.sigma_BNInit=[[0.1],[0.2],[-0.3]]# sigma_BN_BscObject.hub.omega_BN_BInit=[[0.000],[-0.00],[0.00]]# rad/s - omega_BN_B# Set up the extForceTorque moduleextFTObject=extForceTorque.ExtForceTorque()extFTObject.ModelTag="externalDisturbance"scObject.addDynamicEffector(extFTObject)scSim.AddModelToTask(simTaskName,extFTObject)# Add the simple Navigation sensor module. This sets the SC attitude, rate, position# velocity navigation messagesNavObject=simpleNav.SimpleNav()sNavObject.ModelTag="SimpleNavigation"scSim.AddModelToTask(simTaskName,sNavObject)sNavObject.scStateInMsg.subscribeTo(scObject.scStateOutMsg)## Set up the FSW algorithm tasks## Set up solar panel Sun-pointing guidance modulesunPointGuidance=locationPointing.locationPointing()sunPointGuidance.ModelTag="panelSunPoint"sunPointGuidance.celBodyInMsg.subscribeTo(ephemObject.ephemOutMsgs[sunIdx])sunPointGuidance.scTransInMsg.subscribeTo(sNavObject.transOutMsg)sunPointGuidance.scAttInMsg.subscribeTo(sNavObject.attOutMsg)sunPointGuidance.pHat_B=[0.0,0.0,1.0]sunPointGuidance.useBoresightRateDamping=1scSim.AddModelToTask(simTaskName,sunPointGuidance)# Set up asteroid-relative velocityPoint guidance modulevelAsteroidGuidance=velocityPoint.velocityPoint()velAsteroidGuidance.ModelTag="velocityPointAsteroid"velAsteroidGuidance.transNavInMsg.subscribeTo(sNavObject.transOutMsg)velAsteroidGuidance.celBodyInMsg.subscribeTo(ephemObject.ephemOutMsgs[asteroidIdx])velAsteroidGuidance.mu=muscSim.AddModelToTask(simTaskName,velAsteroidGuidance)# Set up sensor science-pointing guidance modulecameraLocation=[0.0,1.5,0.0]sciencePointGuidance=locationPointing.locationPointing()sciencePointGuidance.ModelTag="sciencePointAsteroid"sciencePointGuidance.celBodyInMsg.subscribeTo(ephemObject.ephemOutMsgs[asteroidIdx])sciencePointGuidance.scTransInMsg.subscribeTo(sNavObject.transOutMsg)sciencePointGuidance.scAttInMsg.subscribeTo(sNavObject.attOutMsg)sciencePointGuidance.pHat_B=cameraLocation# y-axis set for science-pointing sensorsciencePointGuidance.useBoresightRateDamping=1scSim.AddModelToTask(simTaskName,sciencePointGuidance)# Set up an antenna pointing to Earth guidance moduleearthPointGuidance=locationPointing.locationPointing()earthPointGuidance.ModelTag="antennaEarthPoint"earthPointGuidance.celBodyInMsg.subscribeTo(ephemObject.ephemOutMsgs[earthIdx])earthPointGuidance.scTransInMsg.subscribeTo(sNavObject.transOutMsg)earthPointGuidance.scAttInMsg.subscribeTo(sNavObject.attOutMsg)earthPointGuidance.pHat_B=[0.0,0.0,1.0]earthPointGuidance.useBoresightRateDamping=1scSim.AddModelToTask(simTaskName,earthPointGuidance)# Set up the attitude tracking error evaluation moduleattError=attTrackingError.attTrackingError()attError.ModelTag="attErrorInertial3D"scSim.AddModelToTask(simTaskName,attError)attError.attRefInMsg.subscribeTo(sunPointGuidance.attRefOutMsg)# initial flight modeattError.attNavInMsg.subscribeTo(sNavObject.attOutMsg)# Create the FSW vehicle configuration messagevehicleConfigOut=messaging.VehicleConfigMsgPayload()vehicleConfigOut.ISCPntB_B=I# use the same inertia in the FSW algorithm as in the simulationvcMsg=messaging.VehicleConfigMsg().write(vehicleConfigOut)# Set up the MRP Feedback control modulemrpControl=mrpFeedback.mrpFeedback()mrpControl.ModelTag="mrpFeedback"scSim.AddModelToTask(simTaskName,mrpControl)mrpControl.guidInMsg.subscribeTo(attError.attGuidOutMsg)mrpControl.vehConfigInMsg.subscribeTo(vcMsg)mrpControl.Ki=-1.0# make value negative to turn off integral feedbackII=900.mrpControl.P=2*II/(20*60)mrpControl.K=mrpControl.P*mrpControl.P/IImrpControl.integralLimit=2./mrpControl.Ki*0.1# Connect the torque command to external torque effectorextFTObject.cmdTorqueInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg)# Set the simulation time# Set up data logging before the simulation is initializedscRec=scObject.scStateOutMsg.recorder()astRec=gravBodyEphem.planetOutMsgs[0].recorder()scSim.AddModelToTask(simTaskName,scRec)scSim.AddModelToTask(simTaskName,astRec)ifvizSupport.vizFound:# Set up the sensor for the science-pointing modegenericSensor=vizInterface.GenericSensor()genericSensor.r_SB_B=cameraLocationgenericSensor.fieldOfView.push_back(10.0*macros.D2R)genericSensor.fieldOfView.push_back(10.0*macros.D2R)genericSensor.normalVector=cameraLocationgenericSensor.size=10genericSensor.color=vizInterface.IntVector(vizSupport.toRGBA255("white",alpha=0.1))genericSensor.label="scienceCamera"genericSensor.genericSensorCmd=1# Set up the antenna visualization for transmission to EarthtransceiverHUD=vizInterface.Transceiver()transceiverHUD.r_SB_B=[0.,0.,1.38]transceiverHUD.fieldOfView=40.0*macros.D2RtransceiverHUD.normalVector=[0.,0.,1.]transceiverHUD.color=vizInterface.IntVector(vizSupport.toRGBA255("cyan"))transceiverHUD.label="antenna"transceiverHUD.animationSpeed=1# Set up the thruster visualization info# Note: This process is different from the usual procedure of creating a thruster effector.# The following code creates a thruster visualization only.# before adding the thrusterscData=vizInterface.VizSpacecraftData()scData.spacecraftName=scObject.ModelTagscData.scStateInMsg.subscribeTo(scObject.scStateOutMsg)scData.transceiverList=vizInterface.TransceiverVector([transceiverHUD])scData.genericSensorList=vizInterface.GenericSensorVector([genericSensor])thrusterMsgInfo=messaging.THROutputMsgPayload()thrusterMsgInfo.maxThrust=1# NewtonsthrusterMsgInfo.thrustForce=0# NewtonsthrusterMsgInfo.thrusterLocation=[0,0,-1.5]thrusterMsgInfo.thrusterDirection=[0,0,1]thrMsg=messaging.THROutputMsg().write(thrusterMsgInfo)scData.thrInMsgs=messaging.THROutputMsgInMsgsVector([thrMsg.addSubscriber()])thrInfo=vizInterface.ThrClusterMap()thrInfo.thrTag="DV"scData.thrInfo=vizInterface.ThrClusterVector([thrInfo])# Create the Vizard visualization file and set parametersviz=vizSupport.enableUnityVisualization(scSim,simTaskName,scObject# , saveFile=fileName)viz.epochInMsg.subscribeTo(gravFactory.epochMsg)viz.settings.showCelestialBodyLabels=1viz.settings.showSpacecraftLabels=1viz.settings.truePathFixedFrame="bennu"viz.settings.trueTrajectoryLinesOn=5# relative to celestial body fixed frameviz.settings.scViewToPlanetViewBoundaryMultiplier=100viz.settings.planetViewToHelioViewBoundaryMultiplier=100viz.settings.orbitLinesOn=-1viz.settings.keyboardAngularRate=np.deg2rad(0.5)# Create the science mode cameravizSupport.createStandardCamera(viz,setMode=1,spacecraftName=scObject.ModelTag,fieldOfView=10*macros.D2R,displayName="10˚ FOV Camera",pointingVector_B=[0,1,0],position_B=cameraLocation)# Note: After running the enableUnityVisualization() method, we need to clear the# vizInterface spacecraft data container, scData, and push our custom copy to it.viz.scData.clear()viz.scData.push_back(scData)# Initialize and execute the simulation for the first sectionscSim.InitializeSimulation()# Set up flight modesdefrunPanelSunPointing(simTime):nonlocalsimulationTimeattError.attRefInMsg.subscribeTo(sunPointGuidance.attRefOutMsg)ifvizSupport.vizFound:transceiverHUD.transceiverState=0# antenna offgenericSensor.isHidden=1thrusterMsgInfo.thrustForce=0thrMsg.write(thrusterMsgInfo,simulationTime)attError.sigma_R0R=[0,0,0]simulationTime+=macros.sec2nano(simTime)scSim.ConfigureStopTime(simulationTime)scSim.ExecuteSimulation()defrunSensorSciencePointing(simTime):nonlocalsimulationTimeattError.attRefInMsg.subscribeTo(sciencePointGuidance.attRefOutMsg)ifvizSupport.vizFound:transceiverHUD.transceiverState=0# antenna offgenericSensor.isHidden=0thrusterMsgInfo.thrustForce=0thrMsg.write(thrusterMsgInfo,simulationTime)attError.sigma_R0R=[0,0,0]simulationTime+=macros.sec2nano(simTime)scSim.ConfigureStopTime(simulationTime)scSim.ExecuteSimulation()defrunAntennaEarthPointing(simTime):nonlocalsimulationTimeattError.attRefInMsg.subscribeTo(earthPointGuidance.attRefOutMsg)ifvizSupport.vizFound:transceiverHUD.transceiverState=3# antenna in send and receive modegenericSensor.isHidden=1thrusterMsgInfo.thrustForce=0thrMsg.write(thrusterMsgInfo,simulationTime)attError.sigma_R0R=[0,0,0]simulationTime+=macros.sec2nano(simTime)scSim.ConfigureStopTime(simulationTime)scSim.ExecuteSimulation()defrunDvBurn(simTime,burnSign,planetMsg):nonlocalsimulationTimeattError.attRefInMsg.subscribeTo(planetMsg)ifvizSupport.vizFound:transceiverHUD.transceiverState=0# antenna offgenericSensor.isHidden=1ifburnSign>0:attError.sigma_R0R=[np.tan((np.pi/2)/4),0,0]else:attError.sigma_R0R=[-np.tan((np.pi/2)/4),0,0]minTime=40*60ifsimTime<minTime:print("ERROR: runPosDvBurn must have simTime larger than "+str(minTime)+" min")exit(1)else:simulationTime+=macros.sec2nano(minTime)scSim.ConfigureStopTime(simulationTime)scSim.ExecuteSimulation()ifvizSupport.vizFound:thrusterMsgInfo.thrustForce=thrusterMsgInfo.maxThrustthrMsg.write(thrusterMsgInfo,simulationTime)simulationTime+=macros.sec2nano(simTime-minTime)scSim.ConfigureStopTime(simulationTime)scSim.ExecuteSimulation()simulationTime=0np.set_printoptions(precision=16)burnTime=200*60# Run thruster burn for arrival to the capture orbit with thrusters onrunDvBurn(T1,-1,velAsteroidGuidance.attRefOutMsg)# Get current spacecraft statesvelRef=scObject.dynManager.getStateObject(scObject.hub.nameOfHubVelocity)vN=scRec.v_BN_N[-1]-astRec.VelocityVector[-1]# Apply a delta V and set the new velocity state in the circular capture orbitvHat=vN/np.linalg.norm(vN)vN=vN+Delta_V_Parking_Orbit*vHatvelRef.setState(vN)# Travel in a circular orbit at r0, incorporating several attitude pointing modesrunDvBurn(burnTime,-1,velAsteroidGuidance.attRefOutMsg)runSensorSciencePointing(P0/3.-burnTime)runPanelSunPointing(P0/3.)runAntennaEarthPointing(P0/3.-burnTime)runDvBurn(burnTime,-1,velAsteroidGuidance.attRefOutMsg)# Get access to dynManager translational states for future access to the statesvelRef=scObject.dynManager.getStateObject(scObject.hub.nameOfHubVelocity)# Retrieve the latest relative position and velocity componentsrN=scRec.r_BN_N[-1]-astRec.PositionVector[-1]vN=scRec.v_BN_N[-1]-astRec.VelocityVector[-1]# Conduct the first burn of a Hohmann transfer from r0 to r1rData=np.linalg.norm(rN)vData=np.linalg.norm(vN)at=(rData+r1)*.5v0p=np.sqrt((2*mu/rData)-(mu/at))vHat=vN/vDatavVt=vN+vHat*(v0p-vData)# Update state manager's velocityvelRef.setState(vVt)# Run thruster burn mode along with sun-pointing during the transfer orbitrunDvBurn(burnTime,-1,velAsteroidGuidance.attRefOutMsg)runPanelSunPointing(P01/2.-burnTime*2)runDvBurn(burnTime,-1,velAsteroidGuidance.attRefOutMsg)# Retrieve the latest relative position and velocity componentsrN=scRec.r_BN_N[-1]-astRec.PositionVector[-1]vN=scRec.v_BN_N[-1]-astRec.VelocityVector[-1]# Conduct the second burn of the Hohmann transfer to arrive in a circular orbit at r1rData=np.linalg.norm(rN)vData=np.linalg.norm(vN)v1p=np.sqrt(mu/rData)vHat=vN/vDatavVt2=vN+vHat*(v1p-vData)# Update state manager's velocityvelRef.setState(vVt2)# Run thruster burn visualization along with attitude pointing modesrunDvBurn(burnTime,-1,velAsteroidGuidance.attRefOutMsg)runSensorSciencePointing(P1/4-burnTime)runAntennaEarthPointing(P1/4-burnTime)runDvBurn(burnTime,-1,velAsteroidGuidance.attRefOutMsg)# Retrieve the latest relative position and velocity componentsrN=scRec.r_BN_N[-1]-astRec.PositionVector[-1]vN=scRec.v_BN_N[-1]-astRec.VelocityVector[-1]# Conduct a second Hohmann transfer from r1 to r2, initial burnrData=np.linalg.norm(rN)vData=np.linalg.norm(vN)at=(rData+r2)*.5v2p=np.sqrt((2*mu/rData)-(mu/at))vHat=vN/vDatavVt=vN+vHat*(v2p-vData)# Update state manager's velocityvelRef.setState(vVt)# Run thruster burn section with science pointing moderunDvBurn(burnTime,-1,velAsteroidGuidance.attRefOutMsg)runSensorSciencePointing(P12/2-burnTime*2)runDvBurn(burnTime,-1,velAsteroidGuidance.attRefOutMsg)# Retrieve the latest relative position and velocity componentsrN=scRec.r_BN_N[-1]-astRec.PositionVector[-1]vN=scRec.v_BN_N[-1]-astRec.VelocityVector[-1]# Conduct the second burn of the second transfer to a cicular orbit at r2rData=np.linalg.norm(rN)vData=np.linalg.norm(vN)v3p=np.sqrt(mu/rData)vHat=vN/vDatavVt=vN+vHat*(v3p-vData)# Update state manager's velocityvelRef.setState(vVt)# Run thruster visualization with science pointing moderunDvBurn(burnTime,-1,velAsteroidGuidance.attRefOutMsg)runSensorSciencePointing(P2-burnTime)# Retrieve the latest relative position and velocity componentsrN=scRec.r_BN_N[-1]-astRec.PositionVector[-1]vN=scRec.v_BN_N[-1]-astRec.VelocityVector[-1]# Conduct a third Hohmann transfer from r2 to r3, initial burnrData=np.linalg.norm(rN)vData=np.linalg.norm(vN)at=(rData+r3)*.5v3p=np.sqrt((2*mu/rData)-(mu/at))vHat=vN/vDatavVt=vN+vHat*(v3p-vData)# Update state manager's velocityvelRef.setState(vVt)# Run thruster visualization with science-pointing moderunDvBurn(burnTime,-1,velAsteroidGuidance.attRefOutMsg)runSensorSciencePointing(3*P23-burnTime)# Retrieve logged spacecraft position relative to asteroidposData1=scRec.r_BN_N# inertial pos. wrt. SunposData2=scRec.r_BN_N-astRec.PositionVector# relative pos. wrt. Asteroid# Call plotting function: plotOrbitsfigureList=plotOrbits(scRec.times(),posData1,posData2,rP,diam)ifshow_plots:plt.show()# Close the plots being saved off to avoid over-writing old and new figuresplt.close("all")# Unload Spice kernelsgravFactory.unloadSpiceKernels()returnfigureList
defplotOrbits(timeAxis,posData1,posData2,rP,diam):fileName=os.path.basename(os.path.splitext(__file__)[0])plt.close("all")# Clears out plots from earlier test runsfigureList={}# Plot arrival to Asteroidplt.figure(1,figsize=(5,5))# Draw the planetfig=plt.gcf()ax=fig.gca()ax.set_aspect('equal')ax.ticklabel_format(useOffset=False,style='sci')ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambdax,loc:"{:,}".format(int(x))))ax.get_xaxis().set_major_formatter(plt.FuncFormatter(lambdax,loc:"{:,}".format(int(x))))planetColor='#008800'planetRadius=.5*(diam)# max.add_artist(plt.Circle((0,0),planetRadius,color=planetColor))# Draw the actual orbit from pulled data (DataRec)plt.plot(posData2[:,0],posData2[:,2],color='orangered',label='Simulated Flight')plt.xlabel('X Distance, Inertial [m]')plt.ylabel('Z Distance, Inertial [m]')# Draw desired parking orbitfData=np.linspace(0,2*np.pi,100)rData=[]forindxinrange(0,len(fData)):rData.append(rP)plt.plot(rData*np.cos(fData),rData*np.sin(fData),'--',color='#555555',label='Desired Circ.Capture Orbit')plt.legend(loc='upper right')plt.grid()pltName=fileName+"1"figureList[pltName]=plt.figure(1)returnfigureListif__name__=="__main__":run(True# show_plots)