#
# 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"""
Overview
--------
Demonstrates how to run basic Monte-Carlo (MC) RW-based attitude simulations.
This script duplicates the scenario in :ref:`scenarioAttitudeFeedbackRW` where a
6-DOF spacecraft is orbiting the Earth. Here some simulation parameters are dispersed randomly
using a multi threaded Monte-Carlo setup. Reaction Wheel (RW) state effector are added
to the rigid spacecraft() hub, and what flight
algorithm module is used to control these RWs. The scenario is run in a single configuration:
by not using the Jitter model and by using the RW Voltage IO. Given this scenario we can add dispersions
to the variables in between each MC run.
The script is found in the folder ``basilisk/examples`` and executed by using::
python3 scenarioMonteCarloAttRW.py
For more information on the Attitude Feedback Simulation with RW, please see the documentation
on the :ref:`scenarioAttitudeFeedbackRW` file.
Enable Terminal Bar to Show Simulation Progress
-----------------------------------------------
To enable progress bar, one need to set ``showProgressBar`` data member of class SimulationParameters to true.
.. code-block:: python
monteCarlo = Controller()
monteCarlo.setShowProgressBar(True)
Method ``setShowProgressBar`` should be used to set variable ``showProgressBar`` as True with the above statement. After
enabling the progress bar, all the simulation run by ``monteCarlo.ExecuteSimulation()`` and
montoCarlo.runInitialConditions will show the progress bar in the terminal.
Setup Changes for Monte-Carlo Runs
----------------------------------
In order to set up the multi-threaded MC simulation, the user must first instantiate the Controller class.
The function that is being simulated is the set in this class (in this case, it's defined in the same file as the
MC scenario). The user can then set other variables such as the number of runs, the dispersion seeds, and number of
cores.
The next important step to setting up the MC runs is to disperse the necessary variables.
The dispersions that are set are listed in the following table:
+----------------+---------------------------+--------------------------------------------------+
| Input | Description of Element | Distribution |
+================+===========================+==================================================+
| Inertial | Using Modified | Uniform for all 3 rotations between [0, 2 pi] |
| attitude | Rodrigues Parameters | |
+----------------+---------------------------+--------------------------------------------------+
| Inertial | Using omega vector | Normal dispersions for each of the rotation |
| rotation rate | | components, each of mean 0 and standard |
| | | deviation 0.25 deg/s |
+----------------+---------------------------+--------------------------------------------------+
| Mass of the | Total Mass of the | Uniform around +/-5% of expected values. |
| hub | spacecraft | Bounds are [712.5, 787.5] |
+----------------+---------------------------+--------------------------------------------------+
| Center of Mass | Position vector offset on | Normally around a mean [0, 0, 1], with standard |
| Offset | the actual center of mass,| deviations of [0.05/3, 0.05/3, 0.1/3] |
| | and its theoretical | |
| | position | |
+----------------+---------------------------+--------------------------------------------------+
| Inertia Tensor | 3x3 inertia tensor. | Normally about mean value of diag(900, 800, 600).|
| | Dispersed by 3 rotations | Each of the 3 rotations are normally distributed |
| | | with angles of mean 0 and standard deviation |
| | | 0.1 deg. |
+----------------+---------------------------+--------------------------------------------------+
| RW axes | The rotation axis for | Normally around a respective means [1,0,0], |
| | each of the 3 wheels | [0,1,0], and [0,0,1] with respective standard |
| | | deviations [0.01/3, 0.005/3, 0.005/3], |
| | | [0.005/3, 0.01/3, 0.005/3], and |
| | | [0.005/3, 0.005/3, 0.01/3]. |
+----------------+---------------------------+--------------------------------------------------+
| RW speeds | The rotation speed for | Uniform around +/-5% of expected values. Bounds |
| | each of the 3 wheels | are [95, 105], [190, 210], and [285, 315] |
+----------------+---------------------------+--------------------------------------------------+
| Voltage to | The gain between the | Uniform around +/-5% of expected values. Bounds |
| Torque Gain | commanded torque and the | are [0.019, 0.021] |
| | actual voltage | |
+----------------+---------------------------+--------------------------------------------------+
Next a retention policy is used to log the desired data. The simulation can now be run.
It returns the failed jobs, which should not occur. When the MC have been executed,
the data can be accessed and tested in different ways.
This is explained in the example python code comments.
.. note::
In these Monte Carlo simulations the retained data is stored as the data array with the time
information added as the first column. This is the same retained data format as used
with BSK 1.x.
Illustration of Simulation Results
----------------------------------
::
saveFigures = False, case = 1, show_plots = True, useDatashader = False
.. image:: /_images/Scenarios/scenarioMonteCarloAttRW_AttitudeError.svg
:align: center
.. image:: /_images/Scenarios/scenarioMonteCarloAttRW_RateTrackingError.svg
:align: center
.. image:: /_images/Scenarios/scenarioMonteCarloAttRW_RWMotorTorque.svg
:align: center
.. image:: /_images/Scenarios/scenarioMonteCarloAttRW_RWSpeed.svg
:align: center
.. image:: /_images/Scenarios/scenarioMonteCarloAttRW_RWVoltage.svg
:align: center
"""
#
# Basilisk Integrated Test
#
# Purpose: Integrated test of the MonteCarlo module. Runs multiple
# scenarioAttitudeFeedbackRW with dispersed initial parameters
#
import inspect
import math
import os
import shutil
import matplotlib.pyplot as plt
import numpy as np
filename = inspect.getframeinfo(inspect.currentframe()).filename
fileNameString = os.path.basename(os.path.splitext(__file__)[0])
path = os.path.dirname(os.path.abspath(filename))
from Basilisk import __path__
bskPath = __path__[0]
# import general simulation support files
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import unitTestSupport # general support file with common unit test functions
from Basilisk.utilities import macros
from Basilisk.utilities import orbitalMotion
# import simulation related support
from Basilisk.simulation import spacecraft
from Basilisk.utilities import simIncludeGravBody
from Basilisk.utilities import simIncludeRW
from Basilisk.simulation import simpleNav
from Basilisk.simulation import reactionWheelStateEffector
from Basilisk.simulation import motorVoltageInterface
# import FSW Algorithm related support
from Basilisk.fswAlgorithms import mrpFeedback
from Basilisk.fswAlgorithms import inertial3D
from Basilisk.fswAlgorithms import attTrackingError
from Basilisk.fswAlgorithms import rwMotorTorque
from Basilisk.utilities import fswSetupRW
from Basilisk.fswAlgorithms import rwMotorVoltage
# import message declarations
from Basilisk.architecture import messaging
from Basilisk.utilities.MonteCarlo.Controller import Controller, RetentionPolicy
from Basilisk.utilities.MonteCarlo.Dispersions import (UniformEulerAngleMRPDispersion, UniformDispersion,
NormalVectorCartDispersion, InertiaTensorDispersion)
NUMBER_OF_RUNS = 4
VERBOSE = True
# Here are the name of some messages that we want to retain or otherwise use
rwMotorTorqueMsgName = "rwMotorTorqueMsg"
guidMsgName = "guidMsg"
transMsgName = "transMsg"
rwSpeedMsgName = "rwSpeedMsg"
voltMsgName = "voltMsg"
rwOutName = ["rw1Msg", "rw2Msg", "rw3Msg"]
# We also will need the simulationTime and samplingTimes
numDataPoints = 500
simulationTime = macros.min2nano(10.)
samplingTime = simulationTime // (numDataPoints-1)
[docs]def run(saveFigures, case, show_plots):
"""
The scenarios can be run with the followings setups parameters:
Args:
saveFigures (bool): flag if the scenario figures should be saved for html documentation
case (int): Case 1 is normal MC, case 2 is initial condition run
show_plots (bool): Determines if the script should display plots
"""
# A MonteCarlo simulation can be created using the `MonteCarlo` module.
# This module is used to execute monte carlo simulations, and access
# retained data from previously executed MonteCarlo runs.
# First, the `Controller` class is used in order to define the simulation
monteCarlo = Controller()
# Every MonteCarlo simulation must define a function that creates the `SimulationBaseClass` to
# execute and returns it. Within this function, the simulation is created and configured
monteCarlo.setSimulationFunction(createScenarioAttitudeFeedbackRW)
# Also, every MonteCarlo simulation must define a function which executes the simulation that was created.
monteCarlo.setExecutionFunction(executeScenario)
# A Monte Carlo simulation must define how many simulation runs to execute
monteCarlo.setExecutionCount(NUMBER_OF_RUNS)
# The simulations can have random seeds of each simulation dispersed randomly
monteCarlo.setShouldDisperseSeeds(True)
monteCarlo.setShowProgressBar(True)
# Optionally set the number of cores to use
# monteCarlo.setThreadCount(PROCESSES)
# Whether to print more verbose information during the run
monteCarlo.setVerbose(VERBOSE)
# We set up where to retain the data to.
dirName = "montecarlo_test" + str(os.getpid())
monteCarlo.setArchiveDir(dirName)
# Statistical dispersions can be applied to initial parameters using the MonteCarlo module
dispMRPInit = 'TaskList[0].TaskModels[0].hub.sigma_BNInit'
dispOmegaInit = 'TaskList[0].TaskModels[0].hub.omega_BN_BInit'
dispMass = 'TaskList[0].TaskModels[0].hub.mHub'
dispCoMOff = 'TaskList[0].TaskModels[0].hub.r_BcB_B'
dispInertia = 'hubref.IHubPntBc_B'
dispRW1Axis = 'RW1.gsHat_B'
dispRW2Axis = 'RW2.gsHat_B'
dispRW3Axis = 'RW3.gsHat_B'
dispRW1Omega = 'RW1.Omega'
dispRW2Omega = 'RW2.Omega'
dispRW3Omega = 'RW3.Omega'
dispVoltageIO_0 = 'rwVoltageIO.voltage2TorqueGain[0]'
dispVoltageIO_1 = 'rwVoltageIO.voltage2TorqueGain[1]'
dispVoltageIO_2 = 'rwVoltageIO.voltage2TorqueGain[2]'
dispList = [dispMRPInit, dispOmegaInit, dispMass, dispCoMOff, dispInertia]
# Add dispersions with their dispersion type
monteCarlo.addDispersion(UniformEulerAngleMRPDispersion(dispMRPInit))
monteCarlo.addDispersion(NormalVectorCartDispersion(dispOmegaInit, 0.0, 0.75 / 3.0 * np.pi / 180))
monteCarlo.addDispersion(UniformDispersion(dispMass, ([750.0 - 0.05*750, 750.0 + 0.05*750])))
monteCarlo.addDispersion(NormalVectorCartDispersion(dispCoMOff, [0.0, 0.0, 1.0], [0.05 / 3.0, 0.05 / 3.0, 0.1 / 3.0]))
monteCarlo.addDispersion(InertiaTensorDispersion(dispInertia, stdAngle=0.1))
monteCarlo.addDispersion(NormalVectorCartDispersion(dispRW1Axis, [1.0, 0.0, 0.0], [0.01 / 3.0, 0.005 / 3.0, 0.005 / 3.0]))
monteCarlo.addDispersion(NormalVectorCartDispersion(dispRW2Axis, [0.0, 1.0, 0.0], [0.005 / 3.0, 0.01 / 3.0, 0.005 / 3.0]))
monteCarlo.addDispersion(NormalVectorCartDispersion(dispRW3Axis, [0.0, 0.0, 1.0], [0.005 / 3.0, 0.005 / 3.0, 0.01 / 3.0]))
monteCarlo.addDispersion(UniformDispersion(dispRW1Omega, ([100.0 - 0.05*100, 100.0 + 0.05*100])))
monteCarlo.addDispersion(UniformDispersion(dispRW2Omega, ([200.0 - 0.05*200, 200.0 + 0.05*200])))
monteCarlo.addDispersion(UniformDispersion(dispRW3Omega, ([300.0 - 0.05*300, 300.0 + 0.05*300])))
monteCarlo.addDispersion(UniformDispersion(dispVoltageIO_0, ([0.2/10. - 0.05 * 0.2/10., 0.2/10. + 0.05 * 0.2/10.])))
monteCarlo.addDispersion(UniformDispersion(dispVoltageIO_1, ([0.2/10. - 0.05 * 0.2/10., 0.2/10. + 0.05 * 0.2/10.])))
monteCarlo.addDispersion(UniformDispersion(dispVoltageIO_2, ([0.2/10. - 0.05 * 0.2/10., 0.2/10. + 0.05 * 0.2/10.])))
# A `RetentionPolicy` is used to define what data from the simulation should be retained. A `RetentionPolicy`
# is a list of messages and variables to log from each simulation run. It also has a callback,
# used for plotting/processing the retained data.
retentionPolicy = RetentionPolicy()
# define the data to retain
retentionPolicy.addMessageLog(rwMotorTorqueMsgName, ["motorTorque"])
retentionPolicy.addMessageLog(guidMsgName, ["sigma_BR", "omega_BR_B"])
retentionPolicy.addMessageLog(transMsgName, ["r_BN_N"])
retentionPolicy.addMessageLog(rwSpeedMsgName, ["wheelSpeeds"])
retentionPolicy.addMessageLog(voltMsgName, ["voltage"])
for msgName in rwOutName:
retentionPolicy.addMessageLog(msgName, ["u_current"])
if show_plots:
# plot data only if show_plots is true, otherwise just retain
retentionPolicy.setDataCallback(plotSim)
if saveFigures:
# plot data only if show_plots is true, otherwise just retain
retentionPolicy.setDataCallback(plotSimAndSave)
monteCarlo.addRetentionPolicy(retentionPolicy)
if case == 1:
# After the monteCarlo run is configured, it is executed.
# This method returns the list of jobs that failed.
failures = monteCarlo.executeSimulations()
assert len(failures) == 0, "No runs should fail"
# Now in another script (or the current one), the data from this simulation can be easily loaded.
# This demonstrates loading it from disk
monteCarloLoaded = Controller.load(dirName)
# Then retained data from any run can then be accessed in the form of a dictionary
# with two sub-dictionaries for messages and variables:
retainedData = monteCarloLoaded.getRetainedData(NUMBER_OF_RUNS-1)
assert retainedData is not None, "Retained data should be available after execution"
assert "messages" in retainedData, "Retained data should retain messages"
assert guidMsgName + ".sigma_BR" in retainedData["messages"], "Retained messages should exist"
# We also can rerun a case using the same parameters and random seeds
# If we rerun a properly set-up run, it should output the same data.
# Here we test that if we rerun the case the data doesn't change
oldOutput = retainedData["messages"][guidMsgName + ".sigma_BR"]
# Rerunning the case shouldn't fail
failed = monteCarloLoaded.reRunCases([NUMBER_OF_RUNS-1])
assert len(failed) == 0, "Should rerun case successfully"
# Now access the newly retained data to see if it changed
retainedData = monteCarloLoaded.getRetainedData(NUMBER_OF_RUNS-1)
newOutput = retainedData["messages"][guidMsgName + ".sigma_BR"]
for k1, v1 in enumerate(oldOutput):
for k2, v2 in enumerate(v1):
assert math.fabs(oldOutput[k1][k2] - newOutput[k1][k2]) < .001, \
"Outputs shouldn't change on runs if random seeds are same"
# We can also access the initial parameters
# The random seeds should differ between runs, so we will test that
params1 = monteCarloLoaded.getParameters(NUMBER_OF_RUNS-1)
params2 = monteCarloLoaded.getParameters(NUMBER_OF_RUNS-2)
assert "TaskList[0].TaskModels[0].RNGSeed" in params1, "random number seed should be applied"
for dispName in dispList:
assert dispName in params1, "dispersion should be applied"
# assert two different runs had different parameters.
assert params1[dispName] != params2[dispName], "dispersion should be different in each run"
# Now we execute our callback for the retained data.
# For this run, that means executing the plot.
# We can plot only runs 4,6,7 overlapped
# monteCarloLoaded.executeCallbacks([4,6,7])
# or execute the plot on all runs
monteCarloLoaded.executeCallbacks()
#########################################################
if case == 2:
# Now run initial conditions
icName = path + "/Support/run_MC_IC"
monteCarlo.setICDir(icName)
monteCarlo.setICRunFlag(True)
numberICs = 3
monteCarlo.setExecutionCount(numberICs)
# Rerunning the case shouldn't fail
runsList = list(range(numberICs))
failed = monteCarlo.runInitialConditions(runsList)
assert len(failed) == 0, "Should run ICs successfully"
# monteCarlo.executeCallbacks([4,6,7])
runsList = list(range(numberICs))
monteCarlo.executeCallbacks(runsList)
# And possibly show the plots
if show_plots:
plt.show()
# close the plots being saved off to avoid over-writing old and new figures
plt.close("all")
# Now we clean up data from this test
os.remove(icName + '/' + 'MonteCarlo.data')
for i in range(numberICs):
os.remove(icName + '/' + 'run' + str(i) + '.data')
assert not os.path.exists(icName + '/' + 'MonteCarlo.data'), "No leftover data should exist after the test"
# Now we clean up data from this test
shutil.rmtree(dirName)
assert not os.path.exists(dirName), "No leftover data should exist after the test"
# And possibly show the plots
if show_plots:
print("Test concluded, showing plots now via matplot...")
plt.show()
# close the plots being saved off to avoid over-writing old and new figures
plt.close("all")
# This function creates the simulation to be executed in parallel.
def createScenarioAttitudeFeedbackRW():
# Create simulation variable names
simTaskName = "simTask"
simProcessName = "simProcess"
# Create a sim module as an empty container
scSim = SimulationBaseClass.SimBaseClass()
#
# create the simulation process
#
dynProcess = scSim.CreateNewProcess(simProcessName)
# create the dynamics task and specify the integration update time
simulationTimeStep = macros.sec2nano(.1)
dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep))
#
# setup the simulation tasks/objects
#
# initialize spacecraft object and set properties
scObject = spacecraft.Spacecraft()
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)
scSim.hubref = scObject.hub
# add spacecraft object to the simulation process
scSim.AddModelToTask(simTaskName, scObject, 1)
rwVoltageIO = motorVoltageInterface.MotorVoltageInterface()
rwVoltageIO.ModelTag = "rwVoltageInterface"
# set module parameters(s)
rwVoltageIO.setGains(np.array([0.2/10.]*3)) # [Nm/V] conversion gain
# Add RW Voltage to sim for dispersion
scSim.rwVoltageIO = rwVoltageIO
# Add test module to runtime call list
scSim.AddModelToTask(simTaskName, rwVoltageIO)
# 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)
#
# add RW devices
#
# Make a fresh RW factory instance, this is critical to run multiple times
rwFactory = simIncludeRW.rwFactory()
# store the RW dynamical model type
varRWModel = messaging.BalancedWheels
# create each RW by specifying the RW type, the spin axis gsHat, plus optional arguments
RW1 = rwFactory.create('Honeywell_HR16'
, [1, 0, 0]
, maxMomentum=50.
, Omega=100. # RPM
, RWModel= varRWModel
)
RW2 = rwFactory.create('Honeywell_HR16'
, [0, 1, 0]
, maxMomentum=50.
, Omega=200. # RPM
, RWModel= varRWModel
)
RW3 = rwFactory.create('Honeywell_HR16'
, [0, 0, 1]
, maxMomentum=50.
, Omega=300. # RPM
, rWB_B = [0.5, 0.5, 0.5] # meters
, RWModel= varRWModel
)
numRW = rwFactory.getNumOfDevices()
# create RW object container and tie to spacecraft object
rwStateEffector = reactionWheelStateEffector.ReactionWheelStateEffector()
rwFactory.addToSpacecraft(scObject.ModelTag, rwStateEffector, scObject)
rwStateEffector.rwMotorCmdInMsg.subscribeTo(rwVoltageIO.motorTorqueOutMsg)
# Add RWs to sim for dispersion
scSim.RW1 = RW1
scSim.RW2 = RW2
scSim.RW3 = RW3
# add RW object array to the simulation process
scSim.AddModelToTask(simTaskName, rwStateEffector, 2)
# 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)
#
# setup the FSW algorithm tasks
#
# 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
vcMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut)
# FSW RW configuration message
# use the same RW states in the FSW algorithm as in the simulation
fswSetupRW.clearSetup()
for key, rw in rwFactory.rwList.items():
fswSetupRW.create(unitTestSupport.EigenVector3d2np(rw.gsHat_B), rw.Js, 0.2)
fswRwConfMsg = fswSetupRW.writeConfigMessage()
# setup inertial3D guidance module
inertial3DObj = inertial3D.inertial3D()
inertial3DObj.ModelTag = "inertial3D"
scSim.AddModelToTask(simTaskName, inertial3DObj)
inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation
# setup the attitude tracking error evaluation module
attError = attTrackingError.attTrackingError()
attError.ModelTag = "attErrorInertial3D"
scSim.AddModelToTask(simTaskName, attError)
attError.attRefInMsg.subscribeTo(inertial3DObj.attRefOutMsg)
attError.attNavInMsg.subscribeTo(sNavObject.attOutMsg)
# setup the MRP Feedback control module
mrpControl = mrpFeedback.mrpFeedback()
mrpControl.ModelTag = "mrpFeedback"
scSim.AddModelToTask(simTaskName, mrpControl)
mrpControl.guidInMsg.subscribeTo(attError.attGuidOutMsg)
mrpControl.vehConfigInMsg.subscribeTo(vcMsg)
mrpControl.rwParamsInMsg.subscribeTo(fswRwConfMsg)
mrpControl.rwSpeedsInMsg.subscribeTo(rwStateEffector.rwSpeedOutMsg)
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
# add module that maps the Lr control torque into the RW motor torques
rwMotorTorqueObj = rwMotorTorque.rwMotorTorque()
rwMotorTorqueObj.ModelTag = "rwMotorTorque"
scSim.AddModelToTask(simTaskName, rwMotorTorqueObj)
# Initialize the test module msg names
rwMotorTorqueObj.vehControlInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg)
rwMotorTorqueObj.rwParamsInMsg.subscribeTo(fswRwConfMsg)
# Make the RW control all three body axes
controlAxes_B = [
1,0,0
,0,1,0
,0,0,1
]
rwMotorTorqueObj.controlAxes_B = controlAxes_B
fswRWVoltage = rwMotorVoltage.rwMotorVoltage()
fswRWVoltage.ModelTag = "rwMotorVoltage"
# Add test module to runtime call list
scSim.AddModelToTask(simTaskName, fswRWVoltage)
# Initialize the test module configuration data
fswRWVoltage.torqueInMsg.subscribeTo(rwMotorTorqueObj.rwMotorTorqueOutMsg)
fswRWVoltage.rwParamsInMsg.subscribeTo(fswRwConfMsg)
rwVoltageIO.motorVoltageInMsg.subscribeTo(fswRWVoltage.voltageOutMsg)
# set module parameters
fswRWVoltage.VMin = 0.0 # Volts
fswRWVoltage.VMax = 10.0 # Volts
#
# 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.1], [0.2], [-0.3]] # sigma_CN_B
scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_CN_B
# store the msg recorder modules in a dictionary list so the retention policy class can pull the data
# when the simulation ends
scSim.msgRecList = {}
scSim.msgRecList[rwMotorTorqueMsgName] = rwMotorTorqueObj.rwMotorTorqueOutMsg.recorder(samplingTime)
scSim.AddModelToTask(simTaskName, scSim.msgRecList[rwMotorTorqueMsgName])
scSim.msgRecList[guidMsgName] = attError.attGuidOutMsg.recorder(samplingTime)
scSim.AddModelToTask(simTaskName, scSim.msgRecList[guidMsgName])
scSim.msgRecList[transMsgName] = sNavObject.transOutMsg.recorder(samplingTime)
scSim.AddModelToTask(simTaskName, scSim.msgRecList[transMsgName])
scSim.msgRecList[rwSpeedMsgName] = rwStateEffector.rwSpeedOutMsg.recorder(samplingTime)
scSim.AddModelToTask(simTaskName, scSim.msgRecList[rwSpeedMsgName])
scSim.msgRecList[voltMsgName] = fswRWVoltage.voltageOutMsg.recorder(samplingTime)
scSim.AddModelToTask(simTaskName, scSim.msgRecList[voltMsgName])
c = 0
for msgName in rwOutName:
scSim.msgRecList[msgName] = rwStateEffector.rwOutMsgs[c].recorder(samplingTime)
scSim.AddModelToTask(simTaskName, scSim.msgRecList[msgName])
c += 1
# This is a hack because of a bug in Basilisk... leave this line it keeps
# variables from going out of scope after this function returns
scSim.additionalReferences = [rwVoltageIO, fswRWVoltage, scObject, earth, rwMotorTorqueObj, mrpControl, attError, inertial3DObj]
return scSim
def executeScenario(sim):
# initialize Simulation
sim.InitializeSimulation()
# configure a simulation stop time and execute the simulation run
sim.ConfigureStopTime(simulationTime)
sim.ExecuteSimulation()
# This method is used to plot the retained data of a simulation.
# It is called once for each run of the simulation, overlapping the plots
def plotSim(data, retentionPolicy):
# retrieve the logged data
dataUsReq = data["messages"][rwMotorTorqueMsgName + ".motorTorque"][:,1:]
dataSigmaBR = data["messages"][guidMsgName + ".sigma_BR"][:,1:]
dataOmegaBR = data["messages"][guidMsgName + ".omega_BR_B"][:,1:]
dataPos = data["messages"][transMsgName + ".r_BN_N"][:,1:]
dataOmegaRW = data["messages"][rwSpeedMsgName + ".wheelSpeeds"][:,1:]
dataVolt = data["messages"][voltMsgName + ".voltage"][:,1:]
dataRW = []
for msgName in rwOutName:
dataRW.append(data["messages"][msgName+".u_current"][:,1:])
np.set_printoptions(precision=16)
#
# plot the results
#
timeData = data["messages"][rwMotorTorqueMsgName + ".motorTorque"][:,0] * macros.NANO2MIN
figureList = {}
plt.figure(1)
pltName = 'AttitudeError'
for idx in range(3):
plt.plot(timeData, dataSigmaBR[:, idx],
label='Run ' + str(data["index"]) + r' $\sigma_'+str(idx)+'$')
# plt.legend(loc='lower right')
plt.xlabel('Time [min]')
plt.ylabel(r'Attitude Error $\sigma_{B/R}$')
figureList[pltName] = plt.figure(1)
plt.figure(2)
pltName = 'RWMotorTorque'
for idx in range(3):
plt.plot(timeData, dataUsReq[:, idx],
'--',
label='Run ' + str(data["index"]) + r' $\hat u_{s,'+str(idx)+'}$')
plt.plot(timeData, dataRW[idx],
label='Run ' + str(data["index"]) + ' $u_{s,' + str(idx) + '}$')
# plt.legend(loc='lower right')
plt.xlabel('Time [min]')
plt.ylabel('RW Motor Torque (Nm)')
figureList[pltName] = plt.figure(2)
plt.figure(3)
pltName = 'RateTrackingError'
for idx in range(3):
plt.plot(timeData, dataOmegaBR[:, idx],
label='Run ' + str(data["index"]) + r' $\omega_{BR,'+str(idx)+'}$')
# plt.legend(loc='lower right')
plt.xlabel('Time [min]')
plt.ylabel('Rate Tracking Error (rad/s) ')
figureList[pltName] = plt.figure(3)
plt.figure(4)
pltName = 'RWSpeed'
for idx in range(len(rwOutName)):
plt.plot(timeData, dataOmegaRW[:, idx]/macros.RPM,
label='Run ' + str(data["index"]) + r' $\Omega_{'+str(idx)+'}$')
# plt.legend(loc='lower right')
plt.xlabel('Time [min]')
plt.ylabel('RW Speed (RPM) ')
figureList[pltName] = plt.figure(4)
plt.figure(5)
pltName = 'RWVoltage'
for idx in range(len(rwOutName)):
plt.plot(timeData, dataVolt[:, idx],
label='Run ' + str(data["index"]) + ' $V_{' + str(idx) + '}$')
# plt.legend(loc='lower right')
plt.xlabel('Time [min]')
plt.ylabel('RW Voltage (V) ')
figureList[pltName] = plt.figure(5)
return figureList
def plotSimAndSave(data, retentionPolicy):
figureList = plotSim(data, retentionPolicy)
for pltName, plt in list(figureList.items()):
# plt.subplots_adjust(top = 0.6, bottom = 0.4)
unitTestSupport.saveScenarioFigure(
fileNameString + "_" + pltName
, plt, path + "/dataForExamples")
return
#
# This statement below ensures that the unit test script can be run as a
# # stand-along python script
#
if __name__ == "__main__":
run( saveFigures=False # save figures to file
, case=2 # Case 1 is normal MC, case 2 is initial condition run
, show_plots=True # show_plots.
)