Source code for scenarioAttLocPoint

#
#  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.
#

r"""

.. raw:: html

    <iframe width="560" height="315" src="https://www.youtube.com/embed/AQAcHAmxcaU" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

Overview
--------

Discusses how to use guidance modules to point a particular spacecraft axis towards an Earth
fixed location, Boulder in this example.
This script sets up a 6-DOF spacecraft which is orbiting the Earth.

The script is found in the folder ``basilisk/examples`` and executed by using::

    python3 scenarioAttLocPoint.py

The simulation uses :ref:`groundLocation` to create an output message with Boulder's inertial
position.  This is fed to the 2D pointing module :ref:`locationPointing` which
directs the 3rd body axis to point towards Boulder.

Illustration of Simulation Results
----------------------------------

::

    show_plots = True

The following 2 plots illustrate the 2D pointing error and the external attitude control torque vector components.

.. image:: /_images/Scenarios/scenarioAttLocPoint1.svg
   :align: center

.. image:: /_images/Scenarios/scenarioAttLocPoint2.svg
   :align: center

"""

#
# Basilisk Scenario Script and Integrated Test
#
# Purpose:  Integrated test of the spacecraft(), extForceTorque, simpleNav(),
#           locationPoint() modules.  Will point a spacecraft axis at an Earth fixed location.
# Author:   Hanspeter Schaub
# Creation Date:  May 9, 2021
#

import os

import matplotlib.pyplot as plt
import numpy as np
# The path to the location of Basilisk
# Used to get the location of supporting data.
from Basilisk import __path__
# import message declarations
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import locationPointing
# import FSW Algorithm related support
from Basilisk.fswAlgorithms import mrpFeedback
from Basilisk.simulation import extForceTorque
from Basilisk.simulation import groundLocation
from Basilisk.simulation import simpleNav
# import simulation related support
from Basilisk.simulation import spacecraft
# import general simulation support files
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  # general support file with common unit test functions
from Basilisk.architecture import astroConstants
# attempt to import vizard
from Basilisk.utilities import vizSupport

bskPath = __path__[0]
fileName = os.path.basename(os.path.splitext(__file__)[0])


# Plotting functions
[docs] def plot_attitude_error(timeLineSet, dataSigmaBR): """Plot the attitude result.""" plt.figure(1) fig = plt.gcf() ax = fig.gca() vectorData = dataSigmaBR sNorm = np.array([np.linalg.norm(v) for v in vectorData]) plt.plot(timeLineSet, sNorm, color=unitTestSupport.getLineColor(1, 3), ) plt.xlabel('Time [min]') plt.ylabel(r'Attitude Error Norm $|\sigma_{B/R}|$') ax.set_yscale('log')
[docs] def plot_control_torque(timeLineSet, dataLr): """Plot the control torque response.""" plt.figure(2) for idx in range(3): plt.plot(timeLineSet, dataLr[:, idx], color=unitTestSupport.getLineColor(idx, 3), label='$L_{r,' + str(idx) + '}$') plt.legend(loc='lower right') plt.xlabel('Time [min]') plt.ylabel('Control Torque $L_r$ [Nm]')
[docs] def plot_rate_error(timeLineSet, dataOmegaBR): """Plot the body angular velocity tracking error.""" plt.figure(3) for idx in range(3): plt.plot(timeLineSet, dataOmegaBR[:, idx], color=unitTestSupport.getLineColor(idx, 3), label=r'$\omega_{BR,' + str(idx) + '}$') plt.legend(loc='lower right') plt.xlabel('Time [min]') plt.ylabel('Rate Tracking Error [rad/s] ') return
[docs] def run(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 names simTaskName = "simTask" simProcessName = "simProcess" # Create a sim module as an empty container scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on simulationTime = macros.min2nano(20.) # # create the simulation process # dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time simulationTimeStep = macros.sec2nano(1.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # # setup the simulation tasks/objects # # initialize spacecraft object and set properties scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia I = [900., 0., 0., 0., 800., 0., 0., 0., 600.] scObject.hub.mHub = 750.0 # kg - spacecraft mass scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process scSim.AddModelToTask(simTaskName, scObject) # clear prior gravitational body and SPICE setup definitions gravFactory = simIncludeGravBody.gravBodyFactory() # setup Earth Gravity Body earth = gravFactory.createEarth() earth.isCentralBody = True # ensure this is the central gravitational body mu = earth.mu # attach gravity model to spacecraft gravFactory.addBodiesTo(scObject) # # initialize Spacecraft States with initialization variables # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() oe.a = (6378 + 600)*1000. # meters oe.e = 0.1 oe.i = 63.3 * macros.D2R oe.Omega = 88.2 * macros.D2R oe.omega = 347.8 * macros.D2R oe.f = 135.3 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) scObject.hub.r_CN_NInit = rN # m - r_CN_N scObject.hub.v_CN_NInit = vN # m/s - v_CN_N scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B # setup extForceTorque module # the control torque is read in through the messaging system extFTObject = extForceTorque.ExtForceTorque() extFTObject.ModelTag = "externalDisturbance" # use the input flag to determine which external torque should be applied # Note that all variables are initialized to zero. Thus, not setting this # vector would leave it's components all zero for the simulation. scObject.addDynamicEffector(extFTObject) scSim.AddModelToTask(simTaskName, extFTObject) # add the simple Navigation sensor module. This sets the SC attitude, rate, position # velocity navigation message sNavObject = simpleNav.SimpleNav() sNavObject.ModelTag = "SimpleNavigation" scSim.AddModelToTask(simTaskName, sNavObject) sNavObject.scStateInMsg.subscribeTo(scObject.scStateOutMsg) # Create the ground location groundStation = groundLocation.GroundLocation() groundStation.ModelTag = "BoulderGroundStation" groundStation.planetRadius = astroConstants.REQ_EARTH*1e3 # meters groundStation.specifyLocation(np.radians(40.009971), np.radians(-105.243895), 1624) groundStation.minimumElevation = np.radians(10.) groundStation.maximumRange = 1e9 # meters groundStation.addSpacecraftToModel(scObject.scStateOutMsg) scSim.AddModelToTask(simTaskName, groundStation) # # setup the FSW algorithm tasks # # setup Boulder pointing guidance module locPoint = locationPointing.locationPointing() locPoint.ModelTag = "locPoint" scSim.AddModelToTask(simTaskName, locPoint) locPoint.pHat_B = [0, 0, 1] locPoint.scAttInMsg.subscribeTo(sNavObject.attOutMsg) locPoint.useBoresightRateDamping = 1 locPoint.scTransInMsg.subscribeTo(sNavObject.transOutMsg) locPoint.locationInMsg.subscribeTo(groundStation.currentGroundStateOutMsg) # grMsgData = messaging.GroundStateMsgPayload() # grMsg = messaging.GroundStateMsg().write(grMsgData) # locPoint.locationInMsg.subscribeTo(grMsg) # setup the MRP Feedback control module mrpControl = mrpFeedback.mrpFeedback() mrpControl.ModelTag = "mrpFeedback" scSim.AddModelToTask(simTaskName, mrpControl) mrpControl.guidInMsg.subscribeTo(locPoint.attGuidOutMsg) mrpControl.K = 5.5 mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 # connect torque command to external torque effector extFTObject.cmdTorqueInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg) # # Setup data logging before the simulation is initialized # numDataPoints = 100 samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) mrpLog = mrpControl.cmdTorqueOutMsg.recorder(samplingTime) attErrLog = locPoint.attGuidOutMsg.recorder(samplingTime) snAttLog = sNavObject.attOutMsg.recorder(samplingTime) snTransLog = sNavObject.transOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, mrpLog) scSim.AddModelToTask(simTaskName, attErrLog) scSim.AddModelToTask(simTaskName, snAttLog) scSim.AddModelToTask(simTaskName, snTransLog) # # create simulation messages # # create the FSW vehicle configuration message vehicleConfigOut = messaging.VehicleConfigMsgPayload() vehicleConfigOut.ISCPntB_B = I # use the same inertia in the FSW algorithm as in the simulation configDataMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) mrpControl.vehConfigInMsg.subscribeTo(configDataMsg) # if this scenario is to interface with the BSK Viz, uncomment the following lines if vizSupport.vizFound: viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject # , saveFile=fileName ) vizSupport.addLocation(viz, stationName="Boulder Station" , parentBodyName=earth.displayName , r_GP_P=unitTestSupport.EigenVector3d2list(groundStation.r_LP_P_Init) , fieldOfView=np.radians(160.) , color='pink' , range=2000.0*1000 # meters ) viz.settings.spacecraftSizeMultiplier = 1.5 viz.settings.showLocationCommLines = 1 viz.settings.showLocationCones = 1 viz.settings.showLocationLabels = 1 # # initialize Simulation # scSim.InitializeSimulation() # # configure a simulation stop time and execute the simulation run # scSim.ConfigureStopTime(simulationTime) scSim.ExecuteSimulation() # # retrieve the logged data # dataLr = mrpLog.torqueRequestBody dataSigmaBR = attErrLog.sigma_BR dataOmegaBR = attErrLog.omega_BR_B np.set_printoptions(precision=16) # # plot the results # timeLineSet = attErrLog.times() * macros.NANO2MIN plt.close("all") # clears out plots from earlier test runs plot_attitude_error(timeLineSet, dataSigmaBR) figureList = {} pltName = fileName + "1" figureList[pltName] = plt.figure(1) plot_control_torque(timeLineSet, dataLr) pltName = fileName + "2" figureList[pltName] = plt.figure(2) plot_rate_error(timeLineSet, dataOmegaBR) if show_plots: plt.show() # close the plots being saved off to avoid over-writing old and new figures plt.close("all") return figureList
# # This statement below ensures that the unit test scrip can be run as a # stand-along python script # if __name__ == "__main__": run( True # show_plots )