#
# ISC License
#
# Copyright (c) 2024, 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 scenario illustrates how to add a RW temperature sensor and include some random noise
in the temperature measurement. The RW-based attitude control component is based on
:ref:`scenarioAttitudeFeedbackRW`. This scenario uses the :ref:`motorThermal` module model
the true temperature data from reaction wheels and :ref:`tempMeasurement` module for
adding noise into the temperature readings.
.. caution::
For the :ref:`tempMeasurement` module to provide random noise it is critical that the module
variable ``RNGSeed`` is set to a unique value. Setting it to the same value will result
in the same random numbers being generated each run.
Illustration of Simulation Results
----------------------------------
::
show_plots = True
The following plots illustrate the true temperature of the RWs and the measurement with noise.
.. image:: /_images/Scenarios/scenarioTempMeasurementAttitude1.svg
:align: center
.. image:: /_images/Scenarios/scenarioTempMeasurementAttitude2.svg
:align: center
"""
#
# Basilisk Scenario Script and Integrated Test
#
# Purpose: This scenario illustrates the use of tempMeasurement module and generating random noise in the measurement.
# Author: Yumeka Nagano
# Creation Date: May 10, 2024
#
import os
import matplotlib.pyplot as plt
import numpy as np
import time
# The path to the location of Basilisk
# Used to get the location of supporting data.
from Basilisk import __path__
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import (mrpFeedback, attTrackingError,
inertial3D, rwMotorTorque)
from Basilisk.simulation import (reactionWheelStateEffector, simpleNav,
spacecraft, motorThermal, tempMeasurement)
from Basilisk.utilities import simHelpers
from Basilisk.utilities import (
SimulationBaseClass,
fswSetupRW,
macros,
orbitalMotion,
simIncludeGravBody,
simIncludeRW,
vizSupport,
)
bskPath = __path__[0]
fileName = os.path.basename(os.path.splitext(__file__)[0])
# Plotting functions
[docs]
def plot_rw_temperature(timeData, dataTemp, numRW, id=None):
"""Plot the reaction wheel temperatures"""
plt.figure(id)
for idx in range(numRW):
plt.plot(timeData, dataTemp[:,idx],
color=simHelpers.getLineColor(idx, numRW),
label='$T_{rw,' + str(idx+1) + '}$')
plt.legend(loc='lower right')
plt.xlabel('Time [min]')
plt.ylabel('RW Temperatures [ºC]')
return
[docs]
def plot_rw_temp_measurement(timeData, dataTemp, numRW, id=None):
"""Plot the reaction wheel temperature measurements"""
plt.figure(id)
for idx in range(numRW):
plt.plot(timeData, dataTemp[:,idx],
color=simHelpers.getLineColor(idx, numRW),
label='$T_{rw,' + str(idx+1) + '}$')
plt.legend(loc='lower right')
plt.xlabel('Time [min]')
plt.ylabel('Temp Measurements [ºC]')
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(10.)
#
# 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 = "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 = simHelpers.np2EigenMatrix3d(I)
# add spacecraft object to the simulation process
scSim.AddModelToTask(simTaskName, scObject, 1)
# 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
)
# In this simulation the RW objects RW1, RW2 or RW3 are not modified further. However, you can over-ride
# any values generate in the `.create()` process using for example RW1.Omega_max = 100. to change the
# maximum wheel speed.
numRW = rwFactory.getNumOfDevices()
# create RW object container and tie to spacecraft object
rwStateEffector = reactionWheelStateEffector.ReactionWheelStateEffector()
rwStateEffector.ModelTag = "RW_cluster"
rwFactory.addToSpacecraft(scObject.ModelTag, rwStateEffector, scObject)
# add RW object array to the simulation process. This is required for the UpdateState() method
# to be called which logs the RW states
scSim.AddModelToTask(simTaskName, rwStateEffector, 2)
# create temperature lists for each RW and create objects for each
rwTempList = []
tempMeasList = []
for item in range(numRW):
rwTempList.append(motorThermal.MotorThermal())
tempMeasList.append(tempMeasurement.TempMeasurement())
# initialize the RW temperature
rwTempList[item].ModelTag = "rwThermals" + str(item)
rwTempList[item].currentTemperature = 20 # [ºC]
rwTempList[item].ambientTemperature = 20 # [ºC]
rwTempList[item].efficiency = 0.7
rwTempList[item].ambientThermalResistance = 5 # Air Thermal Resistance
rwTempList[item].motorHeatCapacity = 50 # Motor (steel) Heat Capacity
# initialize the temperature measurement
tempMeasList[item].ModelTag = "tempMeasurementModel" + str(item)
tempMeasList[item].senBias = 0.0 # [C] bias amount
tempMeasList[item].senNoiseStd = 0.5 # [C] noise standard deviation
tempMeasList[item].walkBounds = 0.1 # [C] noise wald bounds
tempMeasList[item].stuckValue = 0.0 # [C] if the sensor gets stuck, stuck at 10 degrees C
tempMeasList[item].spikeProbability = 0.0 # [-] 30% chance of spiking at each time step
tempMeasList[item].spikeAmount = 0.0 # [-] 10x the actual sensed value if the spike happens
tempMeasList[item].faultState = tempMeasurement.TEMP_FAULT_NOMINAL
# tempMeasList[item].RNGSeed = 123 # Seed number (same seed)
tempMeasList[item].RNGSeed = time.time_ns() % (2**32) # Seed number (random for every run)
# add RW temperature and measurement object array to the simulation process
scSim.AddModelToTask(simTaskName, rwTempList[item], 2)
scSim.AddModelToTask(simTaskName, tempMeasList[item], 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)
#
# setup the FSW algorithm tasks
#
# 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)
# setup 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
# add module that maps the Lr control torque into the RW motor torques
rwMotorTorqueObj = rwMotorTorque.rwMotorTorque()
rwMotorTorqueObj.ModelTag = "rwMotorTorque"
scSim.AddModelToTask(simTaskName, rwMotorTorqueObj)
# Make the RW control all three body axes
controlAxes_B = [
1, 0, 0, 0, 1, 0, 0, 0, 1
]
rwMotorTorqueObj.controlAxes_B = controlAxes_B
#
# Setup data logging before the simulation is initialized
#
numDataPoints = 100
samplingTime = simHelpers.samplingTime(simulationTime, simulationTimeStep, numDataPoints)
# A message is created that stores an array of the true temperature and temperature measurement.
# This is logged here to be plotted later on.
rwTempLogs = []
tempMeasLogs = []
for item in range(numRW):
rwTempLogs.append(rwTempList[item].temperatureOutMsg.recorder(samplingTime))
scSim.AddModelToTask(simTaskName, rwTempLogs[item])
tempMeasLogs.append(tempMeasList[item].tempOutMsg.recorder(samplingTime))
scSim.AddModelToTask(simTaskName, tempMeasLogs[item])
#
# create simulation messages
#
# create the FSW vehicle configuration message
# use the same inertia in the FSW algorithm as in the simulation
vehicleConfigOut = messaging.VehicleConfigMsgPayload(ISCPntB_B=I)
vcMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut)
# set up the FSW RW configuration message.
fswSetupRW.clearSetup()
for key, rw in rwFactory.rwList.items():
fswSetupRW.create(simHelpers.EigenVector3d2np(rw.gsHat_B), rw.Js, 0.2)
fswRwParamMsg = fswSetupRW.writeConfigMessage()
#
# set initial Spacecraft States
#
# set up 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
# if this scenario is to interface with the BSK Viz, uncomment the following lines
viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject
# , saveFile=fileName
, rwEffectorList=rwStateEffector
)
# link messages
sNavObject.scStateInMsg.subscribeTo(scObject.scStateOutMsg)
attError.attNavInMsg.subscribeTo(sNavObject.attOutMsg)
attError.attRefInMsg.subscribeTo(inertial3DObj.attRefOutMsg)
mrpControl.guidInMsg.subscribeTo(attError.attGuidOutMsg)
mrpControl.vehConfigInMsg.subscribeTo(vcMsg)
mrpControl.rwParamsInMsg.subscribeTo(fswRwParamMsg)
mrpControl.rwSpeedsInMsg.subscribeTo(rwStateEffector.rwSpeedOutMsg)
rwMotorTorqueObj.rwParamsInMsg.subscribeTo(fswRwParamMsg)
rwMotorTorqueObj.vehControlInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg)
rwStateEffector.rwMotorCmdInMsg.subscribeTo(rwMotorTorqueObj.rwMotorTorqueOutMsg)
for item in range(numRW):
rwTempList[item].rwStateInMsg.subscribeTo(rwStateEffector.rwOutMsgs[item])
tempMeasList[item].tempInMsg.subscribeTo(rwTempList[item].temperatureOutMsg)
#
# initialize Simulation
#
scSim.InitializeSimulation()
#
# configure a simulation stop time and execute the simulation run
#
scSim.ConfigureStopTime(simulationTime)
scSim.ExecuteSimulation()
#
# retrieve the logged data
#
dataRWTemperature = []
dataTempMeasurement = []
for i in range(numRW):
dataRWTemperature.append(rwTempLogs[i].temperature)
dataTempMeasurement.append(tempMeasLogs[i].temperature)
dataRWTemperature = np.array(dataRWTemperature).T
dataTempMeasurement = np.array(dataTempMeasurement).T
np.set_printoptions(precision=16)
#
# plot the results
#
timeData = rwTempLogs[0].times() * macros.NANO2MIN
plt.close("all") # clears out plots from earlier test runs
figureList = {}
plot_rw_temperature(timeData, dataRWTemperature, numRW)
pltName = fileName + "1"
figureList[pltName] = plt.figure(1)
plot_rw_temp_measurement(timeData, dataTempMeasurement, numRW)
pltName = fileName + "2"
figureList[pltName] = plt.figure(2)
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
)