#
#  ISC License
#
#  Copyright (c) 2021, 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 script sets up a formation flying scenario with two spacecraft. The deputy spacecraft attempts to rendezvous with the
chief using attitude-driven differential drag using the strategy outlined in `this paper <https://arc.aiaa.org/doi/10.2514/1.G004521>`_.
This script is found in the folder ``src/examples`` and executed by using::
      python3 scenarioDragRendezvous
The simulation layout is shown in the following illustration. Two spacecraft are orbiting the earth at
close distance. Perturbations from atmospheric drag, provided by :ref:`exponentialAtmosphere` and
:ref:`facetDragDynamicEffector`,  are implemented by default; the :math:`J_2` gravity perturbation
can also be included. Each spacecraft sends a :ref:`simpleNav`
output message of type :ref:`NavAttMsgPayload` continuously to a :ref:`hillStateConverter` module,
which is assumed to be a part of the deputy (maneuvering)
spacecraft's flight software stack. The :ref:`hillStateConverter` module then writes
a :ref:`hillRelStateMsgPayload`, which is read by the :ref:`hillToAttRef` module implementing
the differential drag attitude guidance law. 
.. image:: /_images/static/scenarioDragRendezvousDiagram.png
   :align: center
Illustration of Simulation Results
----------------------------------
::
        0.0, #   altitude offset (m)
        0.1, #  True anomaly offset (deg)
        1, #    Density multiplier (non-dimensional)
        ctrlType='lqr',
        useJ2=False
In this case, the deputy spacecraft attempts to catch up to a reference set ten kilometers ahead of it
along-track using a static LQR control law, without considering the impact of :math:`J_2` perturbations.
The resulting relative attitude and in-plane Hill trajectory are shown below.
.. image:: /_images/Scenarios/scenarioDragRendezvous_relativeAtt.svg
   :align: center
.. image:: /_images/Scenarios/scenarioDragRendezvous_hillTraj.svg
   :align: center
Time trajectories of the in-plane Hill components of the Deputy are shown here:
.. image:: /_images/Scenarios/scenarioDragRendezvous_hillX.svg
   :align: center
.. image:: /_images/Scenarios/scenarioDragRendezvous_hillY.svg
   :align: center
"""
import os
import time
import matplotlib.pyplot as plt
import numpy as np
from Basilisk import __path__
from Basilisk.fswAlgorithms import hillStateConverter, hillToAttRef, hillPoint
from Basilisk.simulation import spacecraft, facetDragDynamicEffector, simpleNav, exponentialAtmosphere
from Basilisk.utilities import RigidBodyKinematics as rbk
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import macros
from Basilisk.utilities import orbitalMotion
from Basilisk.utilities import simIncludeGravBody
from Basilisk.utilities import unitTestSupport
from Basilisk.utilities import vizSupport
bskPath = __path__[0]
fileName = os.path.basename(os.path.splitext(__file__)[0])
#   Declare some linearized drag HCW dynamics
drag_state_dynamics = [[0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,1.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00],
                     [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,1.000000000000000000e+00,0.000000000000000000e+00],
                     [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00],
                     [4.134628279603025589e-06,0.000000000000000000e+00,0.000000000000000000e+00,-7.178791202675993545e-10,2.347943292785702706e-03,0.000000000000000000e+00],
                     [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,-2.347943292785702706e-03,-1.435758240535198709e-09,0.000000000000000000e+00],
                     [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00]]
drag_ctrl_effects = [[0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00],
                     [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00],
                     [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00],
                     [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00],
                     [0.000000000000000000e+00,0.000000000000000000e+00,4.1681080996120926e-05],
                     [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00]
                     ]
drag_sens_effects = [[0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00],
                     [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00],
                     [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00],
                     [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,-7.178791202675151888e-10,0.000000000000000000e+00,0.000000000000000000e+00],
                     [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,-1.435758240535030378e-09,0.000000000000000000e+00],
                     [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00]]
drag_R_inv = [[1e-8,0,0],
              [0,1e-8,0],
              [0,0,1e-8]]
#   Static LQR gain
path = os.path.dirname(os.path.abspath(__file__))
dataFileName = os.path.join(path, "dataForExamples", "static_lqr_controlGain.npz")
lqr_gain_set = np.load(dataFileName)['arr_0.npy']
fileName = os.path.basename(os.path.splitext(__file__)[0])
[docs]def setup_spacecraft_plant(rN, vN, modelName,):
    """
    Convenience function to set up a spacecraft and its associated dragEffector and simpleNav module.
    :param rN:
    :param vN:
    :return:
    """
    scObject = spacecraft.Spacecraft()
    scObject.ModelTag = modelName
    scObject.hub.mHub = 6.0
    scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]]
    I = [10., 0., 0.,
         0., 9., 0.,
         0., 0., 8.]
    scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I)
    scObject.hub.r_CN_NInit = rN
    scObject.hub.v_CN_NInit = vN
    scNav = simpleNav.SimpleNav()
    scNav.ModelTag = modelName+'_navigator'
    scNav.scStateInMsg.subscribeTo(scObject.scStateOutMsg)
    dragArea = 0.3*0.2
    dragCoeff = 2.2
    normalVector = -rbk.euler3(np.radians(45.)).dot(np.array([0,-1,0]))
    panelLocation = [0,0,0]
    dragEffector = facetDragDynamicEffector.FacetDragDynamicEffector()
    dragEffector.ModelTag = modelName+'_dragEffector'
    dragEffector.addFacet(dragArea, dragCoeff, normalVector, panelLocation)
    scObject.addDynamicEffector(dragEffector)
    return scObject, dragEffector, scNav 
[docs]def drag_simulator(altOffset, trueAnomOffset, densMultiplier, ctrlType='lqr', useJ2=False):
    """
    Basilisk simulation of a two-spacecraft rendezvous using relative-attitude driven differential drag. Includes
    both static gain and desensitized time-varying gain options and the option to use simulated attitude control or
    direct reference inputs.
    Args:
        altOffset - double - deputy altitude offset from the chief ('x' hill direction), meters
        trueAnomOffset - double - deputy true anomaly difference from the chief ('y' direction), degrees
    """
    startTime = time.time()
    print(f"Starting process execution for altOffset = {altOffset}, trueAnomOffset={trueAnomOffset}, densMultiplier={densMultiplier} with {ctrlType} controls...")
    scSim = SimulationBaseClass.SimBaseClass()
    #   Configure simulation container and timestep
    simProcessName = "simProcess"
    dynTaskName = "dynTask"
    fswTaskName = "fswTask"
    simProcess = scSim.CreateNewProcess(simProcessName, 2)
    dynTimeStep = macros.sec2nano(60.0)  # Timestep to evaluate dynamics at
    fswTimeStep = macros.sec2nano(60.0)  # Timestep to evaluate FSW at
    simProcess.addTask(scSim.CreateNewTask(dynTaskName, dynTimeStep))
    simProcess.addTask(scSim.CreateNewTask(fswTaskName, fswTimeStep))
    ##  Configure environmental parameters
    #   Gravity; includes 2-body plus J2.
    gravFactory = simIncludeGravBody.gravBodyFactory()
    gravBodies = gravFactory.createBodies(['earth'])
    gravBodies['earth'].isCentralBody = True
    simIncludeGravBody.loadGravFromFile(bskPath + '/supportData/LocalGravData/GGM03S.txt', gravBodies['earth'].spherHarm, 2)
    gravBodies['earth'].useSphericalHarmParams = useJ2
    # timeInitString = '2021 MAY 04 07:47:48.965 (UTC)'
    # gravFactory.createSpiceInterface(bskPath + '/supportData/EphemerisData/'
    #                                           , timeInitString
    #                                           )
    # gravFactory.spiceObject.zeroBase = 'earth'
    #   Density
    atmosphere = exponentialAtmosphere.ExponentialAtmosphere()
    atmosphere.ModelTag = 'atmosphere'
    # atmosphere.planetPosInMsg.subscribeTo(gravFactory.spiceObject.planetStateOutMsgs[0])
    atmosphere.planetRadius = orbitalMotion.REQ_EARTH*1e3 + 300e3 #   m
    atmosphere.envMinReach = -300e3
    atmosphere.envMaxReach = +300e3
    atmosphere.scaleHeight = 8.0e3    # m
    atmosphere.baseDensity =  2.022E-14 * 1000 *  densMultiplier #    kg/m^3
    ##   Set up chief, deputy orbits:
    mu = gravFactory.gravBodies['earth'].mu
    chief_oe = orbitalMotion.ClassicElements()
    chief_oe.a = orbitalMotion.REQ_EARTH * 1e3 + 300e3 # meters
    chief_oe.e = 0.
    chief_oe.i = np.radians(45.)
    chief_oe.Omega = np.radians(20.0)
    chief_oe.omega = np.radians(30.)
    chief_oe.f = np.radians(20.)
    chief_rN, chief_vN = orbitalMotion.elem2rv(mu, chief_oe)
    dep_oe = orbitalMotion.ClassicElements()
    dep_oe.a = orbitalMotion.REQ_EARTH * 1e3 + 300e3 + (altOffset)  # meters
    dep_oe.e = 0.
    dep_oe.i = np.radians(45.)
    dep_oe.Omega = np.radians(20.0)
    dep_oe.omega = np.radians(30.)
    dep_oe.f = np.radians(20. - trueAnomOffset)
    dep_rN, dep_vN = orbitalMotion.elem2rv(mu, dep_oe)
    #   Initialize s/c dynamics, drag, navigation solutions
    chiefSc, chiefDrag, chiefNav = setup_spacecraft_plant(chief_rN, chief_vN, 'wiggum')
    depSc, depDrag, depNav = setup_spacecraft_plant(dep_rN,dep_vN, 'lou')
    #   Connect s/c to environment (gravity, density)
    chiefSc.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values()))
    depSc.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values()))
    atmosphere.addSpacecraftToModel(depSc.scStateOutMsg)
    depDrag.atmoDensInMsg.subscribeTo(atmosphere.envOutMsgs[-1])
    atmosphere.addSpacecraftToModel(chiefSc.scStateOutMsg)
    chiefDrag.atmoDensInMsg.subscribeTo(atmosphere.envOutMsgs[-1])
    #   Add all dynamics stuff to dynamics task
    scSim.AddModelToTask(dynTaskName, atmosphere,ModelPriority=920)
    # scSim.AddModelToTask(dynTaskName, ephemConverter, 921)
    scSim.AddModelToTask(dynTaskName, chiefDrag,ModelPriority=890)
    scSim.AddModelToTask(dynTaskName, depDrag,ModelPriority=891)
    scSim.AddModelToTask(dynTaskName, chiefSc,ModelPriority=810)
    scSim.AddModelToTask(dynTaskName, depSc,ModelPriority=811)
    scSim.AddModelToTask(dynTaskName, chiefNav,ModelPriority=800)
    scSim.AddModelToTask(dynTaskName, depNav,ModelPriority=801)
    # scSim.AddModelToTask(dynTaskName, gravFactory.spiceObject, 999)
    scSim.AddVariableForLogging(chiefDrag.ModelTag + ".forceExternal_B",
                                      dynTimeStep, 0, 2, 'double')
    scSim.AddVariableForLogging(depDrag.ModelTag + ".forceExternal_B",
                                      dynTimeStep, 0, 2, 'double')
    ##  FSW setup
    #   Chief S/C
    #   hillPoint - set such that the chief attitude follows its hill frame.
    chiefAttRefData = hillPoint.hillPointConfig()
    chiefAttRefWrap = scSim.setModelDataWrap(chiefAttRefData)
    chiefAttRefWrap.ModelTag = 'chief_att_ref'
    chiefAttRefData.transNavInMsg.subscribeTo(chiefNav.transOutMsg)
    # chiefAttRefData.celBodyInMsg.subscribeTo(ephemConverter.ephemOutMsgs[-1]) #   We shouldn't need this because the planet is the origin
    chiefSc.attRefInMsg.subscribeTo(chiefAttRefData.attRefOutMsg) #  Force the chief spacecraft to follow the hill direction
    depHillRefData = hillPoint.hillPointConfig()
    depHillRefWrap = scSim.setModelDataWrap(depHillRefData)
    depHillRefWrap.ModelTag = 'dep_hill_ref'
    depHillRefData.transNavInMsg.subscribeTo(depNav.transOutMsg)
    # chiefAttRefData.celBodyInMsg.subscribeTo(ephemConverter.ephemOutMsgs[-1]) #   We shouldn't need this because the planet is the origin
    #chiefSc.attRefInMsg.subscribeTo(chiefAttRefData.attRefOutMsg) #  Force the chief spacecraft to follow the hill direction
    # hillStateConverter
    hillStateNavData = hillStateConverter.HillStateConverterConfig()
    hillStateNavWrap = scSim.setModelDataWrap(hillStateNavData)
    hillStateNavWrap.ModelTag = "dep_hillStateNav"
    hillStateNavData.depStateInMsg.subscribeTo(depNav.transOutMsg)
    hillStateNavData.chiefStateInMsg.subscribeTo(chiefNav.transOutMsg)
    # hillToAtt guidance law w/ static gain
    depAttRefData = hillToAttRef.HillToAttRefConfig()
    depAttRefWrap = scSim.setModelDataWrap(depAttRefData)
    depAttRefData.ModelTag = 'dep_att_ref'
    depAttRefData.gainMatrix = hillToAttRef.MultiArray(lqr_gain_set)
    #   Configure parameters common to relative attitude guidance modules
    depAttRefData.hillStateInMsg.subscribeTo(hillStateNavData.hillStateOutMsg)
    # depAttRef.attStateInMsg.subscribeTo(chiefNav.attOutMsg)
    depAttRefData.attRefInMsg.subscribeTo(depHillRefData.attRefOutMsg)
    depAttRefData.relMRPMin = -0.2
    depAttRefData.relMRPMax = 0.2
    #   Set the deputy spacecraft to directly follow the attRefMessage
    depSc.attRefInMsg.subscribeTo(depAttRefData.attRefOutMsg)
    scSim.AddModelToTask(dynTaskName, chiefAttRefWrap, chiefAttRefData, 710)
    scSim.AddModelToTask(dynTaskName, hillStateNavWrap, hillStateNavData,790)
    scSim.AddModelToTask(dynTaskName, depHillRefWrap, depHillRefData,789)
    scSim.AddModelToTask(dynTaskName, depAttRefWrap, depAttRefData, 700)
    # ----- log ----- #
    orbit_period = 2*np.pi/np.sqrt(mu/chief_oe.a**3)
    simulationTime = 40*orbit_period#106920.14366466808 
    simulationTime = macros.sec2nano(simulationTime)
    numDataPoints = 21384
    samplingTime = simulationTime // (numDataPoints - 1)
    #   Set up recorders
    chiefStateRec = chiefSc.scStateOutMsg.recorder()
    depStateRec = depSc.scStateOutMsg.recorder()
    hillStateRec = hillStateNavData.hillStateOutMsg.recorder()
    depAttRec = depAttRefData.attRefOutMsg.recorder()
    chiefAttRec = chiefAttRefData.attRefOutMsg.recorder()
    atmoRecs = []
    for msg in atmosphere.envOutMsgs:
        atmoRec = msg.recorder()
        atmoRecs.append(atmoRec)
    scSim.AddModelToTask(dynTaskName, chiefStateRec, ModelPriority=700)
    scSim.AddModelToTask(dynTaskName, depStateRec, ModelPriority=701)
    scSim.AddModelToTask(dynTaskName, hillStateRec, ModelPriority=702)
    scSim.AddModelToTask(dynTaskName, depAttRec, ModelPriority=703)
    scSim.AddModelToTask(dynTaskName, chiefAttRec, ModelPriority=704)
    for ind,rec in enumerate(atmoRecs):
        scSim.AddModelToTask(dynTaskName, rec, 705+ind)
    # if this scenario is to interface with the BSK Viz, uncomment the following lines
    # to save the BSK data to a file, uncomment the saveFile line below
    viz = vizSupport.enableUnityVisualization(scSim, dynTaskName, [chiefSc, depSc],
                                              # saveFile=fileName,
                                              )
    
    # ----- execute sim ----- #
    scSim.InitializeSimulation()
    scSim.ConfigureStopTime(simulationTime)
    setupTimeStamp = time.time()
    setupTime = setupTimeStamp-startTime
    print(f"Sim setup complete in {setupTime} seconds, executing...")
    #scSim.ShowExecutionFigure(True)
    scSim.ExecuteSimulation()
    execTimeStamp = time.time()
    execTime = execTimeStamp - setupTimeStamp
    print(f"Sim complete in {execTime} seconds, pulling data...")
    # ----- pull ----- #
    results_dict = {}
    results_dict['chiefDrag_B'] = scSim.GetLogVariableData(chiefDrag.ModelTag + ".forceExternal_B")
    results_dict['depDrag_B'] = scSim.GetLogVariableData(depDrag.ModelTag + ".forceExternal_B")
    results_dict['dynTimeData'] = chiefStateRec.times()
    results_dict['fswTimeData'] = depAttRec.times()
    results_dict['wiggum.r_BN_N'] = chiefStateRec.r_BN_N 
    results_dict['wiggum.v_BN_N'] = chiefStateRec.v_BN_N
    results_dict['lou.r_BN_N'] = depStateRec.r_BN_N
    results_dict['lou.v_BN_N'] = depStateRec.v_BN_N
    results_dict['relState.r_DC_H'] = hillStateRec.r_DC_H
    results_dict['relState.v_DC_H'] = hillStateRec.v_DC_H
    results_dict['wiggum.sigma_BN'] = chiefStateRec.sigma_BN 
    results_dict['lou.sigma_BN'] = depStateRec.sigma_BN
    results_dict['depDensity'] = atmoRecs[0].neutralDensity
    results_dict['chiefDensity'] = atmoRecs[1].neutralDensity
    results_dict['mu'] = mu
    results_dict['dens_mult'] = densMultiplier
    pullTimeStamp = time.time()
    pullTime = pullTimeStamp - execTimeStamp
    overallTime = pullTimeStamp - startTime
    print(f"Data pulled in {pullTime} seconds. Overall time: {overallTime} seconds.")
    return results_dict 
def run(show_plots, altOffset, trueAnomOffset, densMultiplier, ctrlType='lqr', useJ2=False):
    results = drag_simulator(altOffset, trueAnomOffset, densMultiplier, ctrlType=ctrlType, useJ2=useJ2)
    timeData = results['dynTimeData']
    fswTimeData = results['fswTimeData']
    pos = results['wiggum.r_BN_N']
    vel = results['wiggum.v_BN_N']
    chiefAtt = results['wiggum.sigma_BN']
    pos2 = results['lou.r_BN_N']
    vel2= results['lou.v_BN_N']
    depAtt = results['lou.sigma_BN']
    hillPos = results['relState.r_DC_H']
    hillVel = results['relState.v_DC_H']
    depDensity = results['depDensity']
    chiefDensity = results['chiefDensity']
    depDrag = results['depDrag_B']
    chiefDrag = results['chiefDrag_B']
    densData = results
    numDataPoints = len(timeData)
    mu = results['mu']
    rel_mrp_hist = np.empty([numDataPoints,3])
    for ind in range(0,numDataPoints):
        rel_mrp_hist[ind,:] = rbk.subMRP(depAtt[ind,:], chiefAtt[ind,:])
    figureList = {}
    
    #   Plots for general consumption
    plt.figure() 
    plt.plot(timeData[1:], hillPos[1:,0],label="r_1")
    plt.grid()
    plt.xlabel('Time')   
    plt.ylabel('Hill X Position (m)')
    pltName = fileName + "_hillX"
    figureList[pltName] = plt.figure(1)
    plt.figure()
    plt.plot(timeData[1:], hillPos[1:,1],label="r_2")
    plt.grid()
    plt.xlabel('Time')   
    plt.ylabel('Hill Y Position (m)')
    pltName = fileName + "_hillY"
    figureList[pltName] = plt.figure(2)
    
    plt.figure()
    plt.plot(timeData[1:], hillVel[1:,0],label="v_1")
    plt.grid()
    plt.xlabel('Time')   
    plt.ylabel('Hill X Velocity (m/s)')
    pltName = fileName + "_hilldX"
    figureList[pltName] = plt.figure(3)
    plt.figure()
    plt.plot(timeData[1:], hillVel[1:,1],label="v_2")
    plt.ylabel('Hill Y Velocity (m/s)')
    pltName = fileName + "_hilldy"
    figureList[pltName] = plt.figure(4)
    plt.figure()
    plt.semilogy(timeData[1:], chiefDensity[1:],label=r'Chief $\rho$')
    plt.semilogy(timeData[1:], depDensity[1:],label=r'Deputy $\rho$')
    plt.grid()
    plt.legend()
    plt.xlabel('Time')
    plt.ylabel('Density (kg/m3)')
    pltName = fileName + "_densities"
    figureList[pltName] = plt.figure(5)
    plt.figure()
    plt.plot(hillPos[1:,0],hillPos[1:,1])
    plt.grid()
    plt.xlabel('Hill X (m)')
    plt.ylabel('Hill Y (m)')
    pltName = fileName + "_hillTraj"
    figureList[pltName] = plt.figure(6)
    plt.figure()
    plt.plot(timeData, rel_mrp_hist)
    plt.grid()
    plt.xlabel('Time')
    plt.ylabel('Relative MRP Value')
    pltName = fileName + "_relativeAtt"
    figureList[pltName] = plt.figure(7)
    #   Debug plots
    plt.figure()
    plt.plot(timeData[1:], depDrag[1:,1]-chiefDrag[1:,1],label="delta a_1")
    plt.plot(timeData[1:], depDrag[1:,2]-chiefDrag[1:,2],label="delta a_2")
    plt.plot(timeData[1:], depDrag[1:,3]-chiefDrag[1:,3],label="delta a_3")
    plt.grid()
    plt.legend()
    plt.xlabel('Time')   
    plt.ylabel('Relative acceleration due to drag, body frame (m/s)')
    plt.figure()
    plt.plot(timeData[1:], chiefDrag[1:,1],label="chief a_1")
    plt.plot(timeData[1:], chiefDrag[1:,2],label="chief a_2")
    plt.plot(timeData[1:], chiefDrag[1:,3],label="chief a_3")
    plt.grid()
    plt.legend()
    plt.xlabel('Time')   
    plt.ylabel('Relative acceleration due to drag, body frame (m/s)')
    plt.figure()
    plt.plot(timeData[1:], depDrag[1:,1],label="dep a_1")
    plt.plot(timeData[1:], depDrag[1:,2],label="dep a_2")
    plt.plot(timeData[1:], depDrag[1:,3],label="dep a_3")
    plt.grid()
    plt.legend()
    plt.xlabel('Time')   
    plt.ylabel('Relative acceleration due to drag, body frame (m/s)')
    
    if(show_plots):
        plt.show()
    plt.close("all")
    return figureList
if __name__ == "__main__":
    run(
        True,  # show_plots
        0.0, #   altitude offset (m)
        0.1, #  True anomaly offset (deg)
        1, #    Density multiplier (nondimensional)
        ctrlType='lqr',
        useJ2=False
    )