Source code for scenarioInertialSpiral

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

.. raw:: html

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

Overview
--------

This scenario demonstrates how to layer attitude references. It starts with an inertial pointing attitude and then
adds a spiral scanning motion on top of it.

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

    python3 scenarioInertialSpiral.py

The initial orientation is set to zero for all MRPs and rates, and the desired attitude is layered on top using
:ref:`eulerRotation`, which allows 3-2-1 Euler rotations to be specified. In this script, the first rotation
is 0.02 radians per second about the 1-axis and the second is 0.0001 radians per second about the 2-axis. This
creates an outward sweeping spiral motion. The inertial pointing module feeds its output message into the first
rotation, which feeds into the second, which in turn gives the attitude error that is used by the control law.

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

::

    show_plots = True

Five plots are shown. The first three show the attitude error, control torque, and rate tracking error,
showing a transient response as the initial condition does not match the desired rates.

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

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

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

The final two show the logged MRP data, converted back into Euler angles. The pitch and yaw and plotted
against time as well as against each other to show the resulting spiral.

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

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

"""

#
# Basilisk Scenario Script and Integrated Test
#
# Purpose:  Integrated test of the attitude navigation modules with eulerRotation()
#           module to demonstrate layering attitude references.
# Author:   Galen Bascom
# Creation Date:  February 9, 2022
#

import os

import numpy as np

np.set_printoptions(precision=16)

# import general simulation support files
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import unitTestSupport  # general support file with common unit test functions
import matplotlib.pyplot as plt
from Basilisk.utilities import macros
from Basilisk.utilities import orbitalMotion
from Basilisk.utilities import RigidBodyKinematics

# import simulation related support
from Basilisk.simulation import spacecraft
from Basilisk.simulation import extForceTorque
from Basilisk.utilities import simIncludeGravBody
from Basilisk.simulation import simpleNav

# import FSW Algorithm related support
from Basilisk.fswAlgorithms import mrpFeedback
from Basilisk.fswAlgorithms import inertial3D
from Basilisk.fswAlgorithms import attTrackingError
from Basilisk.fswAlgorithms import eulerRotation

# import message declarations
from Basilisk.architecture import messaging

# The path to the location of Basilisk
# Used to get the location of supporting data.
from Basilisk import __path__
bskPath = __path__[0]
fileName = os.path.basename(os.path.splitext(__file__)[0])


[docs] def run(show_plots): """ The scenario can be run with the followings 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() # Define simulation run time and integration time step simulationTime = macros.min2nano(10.) simulationTimeStep = macros.sec2nano(0.1) # Create the simulation process dynProcess = scSim.CreateNewProcess(simProcessName) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # initialize spacecraft object and set properties scObject = spacecraft.Spacecraft() scObject.ModelTag = "bskSat" # 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) gravFactory = simIncludeGravBody.gravBodyFactory() # se tup 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) # set up extForceTorque module # the control torque is read in through the messaging system extFTObject = 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 message sNavObject = simpleNav.SimpleNav() sNavObject.ModelTag = "SimpleNavigation" scSim.AddModelToTask(simTaskName, sNavObject) # # set up the FSW algorithm tasks # # set up inertial3D guidance module inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(simTaskName, inertial3DObj) inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation # we create 2 dynamic attitude reference modules as we want to do a 1-2 Euler angle rotation # and the modules provide a 3-2-1 sequence. Thus, we do a 0-0-1 321-rotation and then a 0-1-0 321-rotation # get a 1-2 result. attGuidanceEuler1 = eulerRotation.eulerRotation() attGuidanceEuler1.ModelTag = "eulerRotation1" attGuidanceEuler1.attRefInMsg.subscribeTo(inertial3DObj.attRefOutMsg) scSim.AddModelToTask(simTaskName, attGuidanceEuler1) # Make rotation 1 be 0.02 rad/s about 1 axis attGuidanceEuler1.angleRates = [0.0, 0.0, 0.02] attGuidanceEuler2 = eulerRotation.eulerRotation() attGuidanceEuler2.ModelTag = "eulerRotation2" attGuidanceEuler2.attRefInMsg.subscribeTo(attGuidanceEuler1.attRefOutMsg) scSim.AddModelToTask(simTaskName, attGuidanceEuler2) # Make rotation 2 be 0.0001 rad/s about 2 axis attGuidanceEuler2.angleRates = [0.0, 0.0001, 0.0] # set up the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() attError.ModelTag = "attError" scSim.AddModelToTask(simTaskName, attError) # set up the MRP Feedback control module mrpControl = mrpFeedback.mrpFeedback() mrpControl.ModelTag = "mrpFeedback" scSim.AddModelToTask(simTaskName, mrpControl) mrpControl.K = 3.5 mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 # # create simulation messages # configData = messaging.VehicleConfigMsgPayload() configData.ISCPntB_B = I configDataMsg = messaging.VehicleConfigMsg().write(configData) # # connect the messages to the modules # sNavObject.scStateInMsg.subscribeTo(scObject.scStateOutMsg) attError.attNavInMsg.subscribeTo(sNavObject.attOutMsg) attError.attRefInMsg.subscribeTo(attGuidanceEuler2.attRefOutMsg) mrpControl.guidInMsg.subscribeTo(attError.attGuidOutMsg) mrpControl.vehConfigInMsg.subscribeTo(configDataMsg) extFTObject.cmdTorqueInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg) # # Set up data logging before the simulation is initialized # numDataPoints = 100 samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) snAttLog = sNavObject.attOutMsg.recorder(samplingTime) snLog = sNavObject.scStateInMsg.recorder(samplingTime) attErrorLog = attError.attGuidOutMsg.recorder(samplingTime) mrpLog = mrpControl.cmdTorqueOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, snLog) scSim.AddModelToTask(simTaskName, snAttLog) scSim.AddModelToTask(simTaskName, attErrorLog) scSim.AddModelToTask(simTaskName, mrpLog) # # set initial Spacecraft States # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() oe.a = 10000000.0 # meters oe.e = 0.01 oe.i = 33.3 * macros.D2R oe.Omega = 48.2 * macros.D2R oe.omega = 347.8 * macros.D2R oe.f = 85.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.], [0.], [0.]] # sigma_BN_B scObject.hub.omega_BN_BInit = [[0.], [0.], [0.]] # rad/s - omega_BN_B # # initialize Simulation # scSim.InitializeSimulation() # # configure a simulation stop time and execute the simulation run # scSim.ConfigureStopTime(simulationTime) scSim.ExecuteSimulation() dataSigmaBN = snAttLog.sigma_BN dataEulerAnglesPitch = list() dataEulerAnglesYaw = list() dataEulerAnglesRoll = list() for sigma in dataSigmaBN: eulerAngle = RigidBodyKinematics.MRP2Euler321(sigma) dataEulerAnglesPitch.append(eulerAngle[0]) dataEulerAnglesYaw.append(eulerAngle[1]) dataEulerAnglesRoll.append(eulerAngle[2]) timeLineSet = attErrorLog.times() * macros.NANO2MIN # # plot the results # timeAxis = attErrorLog.times() plt.close("all") # clears out plots from earlier test runs plt.figure(1) for idx in range(3): plt.plot(timeAxis * macros.NANO2MIN, attErrorLog.sigma_BR[:, idx], color=unitTestSupport.getLineColor(idx, 3), label=r'$\sigma_' + str(idx) + '$') plt.legend(loc='lower right') plt.xlabel('Time [min]') plt.ylabel(r'Attitude Error $\sigma_{B/R}$') figureList = {} pltName = fileName + "1" figureList[pltName] = plt.figure(1) plt.figure(2) for idx in range(3): plt.plot(timeAxis * macros.NANO2MIN, mrpLog.torqueRequestBody[:, idx], color=unitTestSupport.getLineColor(idx, 3), label='$L_{r,' + str(idx) + '}$') plt.legend(loc='upper right') plt.xlabel('Time [min]') plt.ylabel(r'Control Torque $L_r$ [Nm]') pltName = fileName + "2" figureList[pltName] = plt.figure(2) plt.figure(3) for idx in range(3): plt.plot(timeAxis * macros.NANO2MIN, attErrorLog.omega_BR_B[:, 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] ') pltName = fileName + "3" figureList[pltName] = plt.figure(3) plt.figure(4) plt.plot(timeLineSet, dataEulerAnglesYaw, color=unitTestSupport.getLineColor(0, 3),label=r'Yaw') plt.plot(timeLineSet, dataEulerAnglesPitch, color=unitTestSupport.getLineColor(1, 3),label=r'Pitch') plt.legend(loc='lower right') plt.xlabel('Time [min]') plt.ylabel('Euler Angles [rad]') pltName = fileName + "4" figureList[pltName] = plt.figure(4) plt.figure(5) plt.plot(dataEulerAnglesPitch, dataEulerAnglesYaw) plt.xlabel('Pitch [rad]') plt.ylabel('Yaw [rad]') pltName = fileName + "5" figureList[pltName] = plt.figure(5) 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 )