#
# ISC License
#
# Copyright (c) 2020, 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
--------
Illustrates how to add a :ref:`GravityGradientEffector` to the spacecraft while a Hill-frame pointing
control solution is active. This script expands on :ref:`scenarioAttitudeGuidance` sets
up a 6-DOF spacecraft which is orbiting the Earth. More illustrations on using the gravity gradient
torque effector can be found in the modules :ref:`UnitTestGravityGradientEffector` folder.
The script is found in the folder ``src/examples`` and executed by using::
python3 scenarioAttitudeGG.py
The simulation layout is shown in the following illustration. The :ref:`GravityGradientEffector` is added
to the spacecraft to create a position depended external torque. This
.. image:: /_images/static/test_scenarioAttitudeGG.svg
:align: center
Illustration of Simulation Results
----------------------------------
In this simulation the reference frame is not aligned with the Hill frame, but rather it has a fixed angular
offset along the 2nd body axis. As the Body frame inertia tensor is diagonal, this reference orientation
represents a non-equilibrium gravity gradient torque orientation. Thie simulation results are shown in the
following figures:
.. image:: /_images/Scenarios/scenarioAttitudeGG1.svg
:align: center
.. image:: /_images/Scenarios/scenarioAttitudeGG2.svg
:align: center
.. image:: /_images/Scenarios/scenarioAttitudeGG3.svg
:align: center
Where in :ref:`scenarioAttitudeGuidance` the attitude error asymptotically converged to zero, with the
gravity gradient torque the closed loop dynamics is now only Lagrange stable or bounded.
"""
#
# Basilisk Scenario Script and Integrated Test
#
# Purpose: Integrated test of the spacecraftPlus(), extForceTorque, simpleNav(),
# MRP_Feedback() with attitude navigation modules. Illustrates how
# attitude guidance behavior can be changed in a very modular manner.
# Author: Hanspeter Schaub
# Creation Date: Dec. 2, 2016
#
import os
import numpy as np
# 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 spacecraftPlus
from Basilisk.simulation import extForceTorque
from Basilisk.utilities import simIncludeGravBody
from Basilisk.simulation import simple_nav
from Basilisk.simulation import GravityGradientEffector
# import FSW Algorithm related support
from Basilisk.fswAlgorithms import MRP_Feedback
from Basilisk.fswAlgorithms import hillPoint
from Basilisk.fswAlgorithms import attTrackingError
# import message declarations
from Basilisk.fswAlgorithms import fswMessages
# attempt to import vizard
from Basilisk.utilities import vizSupport
# 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 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(10.)
#
# create the simulation process
#
dynProcess = scSim.CreateNewProcess(simProcessName)
# create the dynamics task and specify the integration update time
simulationTimeStep = macros.sec2nano(0.1)
dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep))
#
# setup the simulation tasks/objects
#
# initialize spacecraftPlus object and set properties
scObject = spacecraftPlus.SpacecraftPlus()
scObject.ModelTag = "spacecraftBody"
# 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 spacecraftPlus 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 spaceCraftPlus
scObject.gravField.gravBodies = spacecraftPlus.GravBodyVector(list(gravFactory.gravBodies.values()))
#
# initialize Spacecraft States with initialization variables
#
# setup the orbit using classical orbit elements
oe = orbitalMotion.ClassicElements()
oe.a = 10000000.0 # meters
oe.e = 0.1
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 = unitTestSupport.np2EigenVectorXd(rN) # m - r_CN_N
scObject.hub.v_CN_NInit = unitTestSupport.np2EigenVectorXd(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
# add gravity gradient effector
ggEff = GravityGradientEffector.GravityGradientEffector()
ggEff.ModelTag = scObject.ModelTag
ggEff.addPlanetName(earth.bodyInMsgName)
scObject.addDynamicEffector(ggEff)
scSim.AddModelToTask(simTaskName, ggEff)
# 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 = simple_nav.SimpleNav()
sNavObject.ModelTag = "SimpleNavigation"
scSim.AddModelToTask(simTaskName, sNavObject)
#
# setup the FSW algorithm tasks
#
# setup hillPoint guidance module
attGuidanceConfig = hillPoint.hillPointConfig()
attGuidanceWrap = scSim.setModelDataWrap(attGuidanceConfig)
attGuidanceWrap.ModelTag = "hillPoint"
attGuidanceConfig.inputNavDataName = sNavObject.outputTransName
# if you want to set attGuidanceConfig.inputCelMessName, then you need a planet ephemeris message of
# type EphemerisIntMsg. In the line below a non-existing message name is used to create an empty planet
# ephemeris message which puts the earth at (0,0,0) origin with zero speed.
attGuidanceConfig.inputCelMessName = "empty_earth_msg"
attGuidanceConfig.outputDataName = "guidanceOut"
scSim.AddModelToTask(simTaskName, attGuidanceWrap, attGuidanceConfig)
# setup the attitude tracking error evaluation module
attErrorConfig = attTrackingError.attTrackingErrorConfig()
attErrorWrap = scSim.setModelDataWrap(attErrorConfig)
attErrorWrap.ModelTag = "attErrorInertial3D"
scSim.AddModelToTask(simTaskName, attErrorWrap, attErrorConfig)
attErrorConfig.outputDataName = "attErrorMsg"
attErrorConfig.inputRefName = attGuidanceConfig.outputDataName
attErrorConfig.inputNavName = sNavObject.outputAttName
attErrorConfig.sigma_R0R = [0, 0.2, 0]
# setup the MRP Feedback control module
mrpControlConfig = MRP_Feedback.MRP_FeedbackConfig()
mrpControlWrap = scSim.setModelDataWrap(mrpControlConfig)
mrpControlWrap.ModelTag = "MRP_Feedback"
scSim.AddModelToTask(simTaskName, mrpControlWrap, mrpControlConfig)
mrpControlConfig.inputGuidName = attErrorConfig.outputDataName
mrpControlConfig.vehConfigInMsgName = "vehicleConfigName"
mrpControlConfig.outputDataName = extFTObject.cmdTorqueInMsgName
mrpControlConfig.K = 3.5
mrpControlConfig.Ki = -1.0 # make value negative to turn off integral feedback
mrpControlConfig.P = 30.0
mrpControlConfig.integralLimit = 2. / mrpControlConfig.Ki * 0.1
#
# Setup data logging before the simulation is initialized
#
numDataPoints = 100
samplingTime = simulationTime // (numDataPoints - 1)
scSim.TotalSim.logThisMessage(mrpControlConfig.outputDataName, samplingTime)
scSim.TotalSim.logThisMessage(attErrorConfig.outputDataName, samplingTime)
scSim.TotalSim.logThisMessage(ggEff.ModelTag + "_gravityGradient", samplingTime) # using default msg name
#
# create simulation messages
#
# create the FSW vehicle configuration message
vehicleConfigOut = fswMessages.VehicleConfigFswMsg()
vehicleConfigOut.ISCPntB_B = I # use the same inertia in the FSW algorithm as in the simulation
unitTestSupport.setMessage(scSim.TotalSim,
simProcessName,
mrpControlConfig.vehConfigInMsgName,
vehicleConfigOut)
# if this scenario is to interface with the BSK Viz, uncomment the following lines
# vizSupport.enableUnityVisualization(scSim, simTaskName, simProcessName, gravBodies=gravFactory, saveFile=fileName)
#
# initialize Simulation
#
scSim.InitializeSimulationAndDiscover()
#
# configure a simulation stop time time and execute the simulation run
#
scSim.ConfigureStopTime(simulationTime)
scSim.ExecuteSimulation()
#
# retrieve the logged data
#
dataLr = scSim.pullMessageLogData(mrpControlConfig.outputDataName + ".torqueRequestBody", list(range(3)))
dataSigmaBR = scSim.pullMessageLogData(attErrorConfig.outputDataName + ".sigma_BR", list(range(3)))
ggData = scSim.pullMessageLogData(ggEff.ModelTag + "_gravityGradient.gravityGradientTorque_B", list(range(3)))
np.set_printoptions(precision=16)
#
# plot the results
#
timeLineSet = dataSigmaBR[:, 0] * macros.NANO2MIN
plt.close("all") # clears out plots from earlier test runs
plt.figure(1)
fig = plt.gcf()
ax = fig.gca()
vectorData = unitTestSupport.pullVectorSetFromData(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')
figureList = {}
pltName = fileName + "1"
figureList[pltName] = plt.figure(1)
plt.figure(2)
fig = plt.gcf()
ax = fig.gca()
vectorData = unitTestSupport.pullVectorSetFromData(dataLr)
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'Control Torque $L_r$ [Nm]$')
ax.set_yscale('log')
pltName = fileName + "2"
figureList[pltName] = plt.figure(2)
plt.figure(3)
for idx in range(1, 4):
plt.plot(ggData[:, 0] * macros.NANO2MIN, ggData[:, idx],
color=unitTestSupport.getLineColor(idx, 3),
label=r'$r_' + str(idx) + '$')
plt.legend(loc='lower right')
plt.xlabel('Time [min]')
plt.ylabel(r'GG Torque [Nm]')
pltName = fileName + "3"
figureList[pltName] = plt.figure(3)
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
)