#
# ISC License
#
# Copyright (c) 2021, Autonomous Vehicle Systems Lab, University of Colorado 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.
#
#
import os
import matplotlib.pyplot as plt
import numpy as np
import pytest
# 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.simulation import MtbEffector
from Basilisk.simulation import spacecraft, magneticFieldWMM
from Basilisk.utilities import SimulationBaseClass, simIncludeGravBody, orbitalMotion, RigidBodyKinematics
from Basilisk.utilities import macros
from Basilisk.utilities import unitTestSupport
bskPath = __path__[0]
fileName = os.path.basename(os.path.splitext(__file__)[0])
def truthMagneticTorque(bField_N, sigmaBN, mtbCmds, GtMatrix, numMTB, maxDipole):
temp = np.asarray(GtMatrix[0:3*numMTB])
GtMatrix = np.reshape(temp, (3, numMTB))
bField_N = np.asarray(bField_N)
BN = RigidBodyKinematics.MRP2C(sigmaBN)
bField_B = BN @ bField_N
for i in range(len(mtbCmds)):
if mtbCmds[i] > maxDipole:
mtbCmds[i] = maxDipole
if mtbCmds[i] < -maxDipole:
mtbCmds[i] = -maxDipole
mtbCmds = np.asarray(mtbCmds[0:numMTB])
bTilde = np.asarray(RigidBodyKinematics.v3Tilde(bField_B))
tauNet = - bTilde @ GtMatrix @ mtbCmds
return tauNet
[docs]
@pytest.mark.parametrize("accuracy", [1e-12])
@pytest.mark.parametrize("maxDipole", [10., 0.1])
def test_MtbEffector(show_plots, accuracy, maxDipole):
r"""
**Validation Test Description**
Compose a general description of what is being tested in this unit test script.
**Test Parameters**
Discuss the test parameters used.
Args:
param1 (int): Dummy test parameter for this parameterized unit test
param2 (int): Dummy test parameter for this parameterized unit test
accuracy (float): absolute accuracy value used in the validation tests
**Description of Variables Being Tested**
Here discuss what variables and states are being checked.
"""
[testResults, testMessage] = MtbEffectorTestFunction(show_plots, accuracy, maxDipole)
assert testResults < 1, testMessage
[docs]
def MtbEffectorTestFunction(show_plots, accuracy, maxDipole):
"""Call this routine directly to run the unit test."""
testFailCount = 0 # zero unit test result counter
testMessages = [] # create empty array to store test log messages
# 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))
simTime = 3.
simulationTime = macros.sec2nano(simTime)
# create Earth Gravity Body
gravFactory = simIncludeGravBody.gravBodyFactory()
earth = gravFactory.createEarth()
earth.isCentralBody = True # ensure this is the central gravitational body
mu = earth.mu
# initialize spacecraft object and set properties
scObject = spacecraft.Spacecraft()
scObject.ModelTag = "bskTestSat"
I = [10.5, 0., 0.,
0., 8., 0.,
0., 0., 6.75]
scObject.hub.mHub = 10.0 # kg - spacecraft mass (arbitrary)
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)
oe = orbitalMotion.ClassicElements()
oe.a = 6778.14 * 1000. # meters
oe.e = 0.0
oe.i = 45. * macros.D2R
oe.Omega = 60. * macros.D2R
oe.omega = 0. * macros.D2R
oe.f = 0. * 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.0], [0.0]] # sigma_CN_B
scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_CN_B
# add spacecraft object
scSim.AddModelToTask(simTaskName, scObject)
scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values()))
# add magnetic field module
magModule = magneticFieldWMM.MagneticFieldWMM()
magModule.ModelTag = "WMM"
magModule.dataPath = bskPath + '/supportData/MagneticField/'
epochMsg = unitTestSupport.timeStringToGregorianUTCMsg('2020 May 12, 00:00:0.0 (UTC)')
magModule.epochInMsg.subscribeTo(epochMsg)
magModule.addSpacecraftToModel(scObject.scStateOutMsg) # this command can be repeated if multiple
scSim.AddModelToTask(simTaskName, magModule)
# add magnetic torque bar effector
mtbEff = MtbEffector.MtbEffector()
mtbEff.ModelTag = "MtbEff"
scObject.addDynamicEffector(mtbEff)
scSim.AddModelToTask(simTaskName, mtbEff)
# mtbConfigData message
mtbConfigParams = messaging.MTBArrayConfigMsgPayload()
mtbConfigParams.numMTB = 3
# row major toque bar alignments
mtbConfigParams.GtMatrix_B = [
1., 0., 0.,
0., 1., 0.,
0., 0., 1.
]
mtbConfigParams.maxMtbDipoles = [maxDipole]*4
mtbParamsInMsg = messaging.MTBArrayConfigMsg().write(mtbConfigParams)
# MTBCmdMsgPayload, mtbCmdInMsg
mtbCmdInMsgContainer = messaging.MTBCmdMsgPayload()
mtbCmdInMsgContainer.mtbDipoleCmds = [0.2, 0.2, 0.2]
mtbCmdInMsg = messaging.MTBCmdMsg().write(mtbCmdInMsgContainer)
# subscribe to mesaages
mtbEff.mtbCmdInMsg.subscribeTo(mtbCmdInMsg)
mtbEff.mtbParamsInMsg.subscribeTo(mtbParamsInMsg)
mtbEff.magInMsg.subscribeTo(magModule.envOutMsgs[0])
# Setup data logging before the simulation is initialized
numDataPoints = 3600
samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints)
dataLog = scObject.scStateOutMsg.recorder(samplingTime)
dataLogMag = magModule.envOutMsgs[0].recorder(samplingTime)
dataLogMTB = mtbEff.mtbOutMsg.recorder(samplingTime)
scSim.AddModelToTask(simTaskName, dataLogMTB)
scSim.AddModelToTask(simTaskName, dataLogMag)
scSim.AddModelToTask(simTaskName, dataLog)
# initialize Simulation
scSim.InitializeSimulation()
# configure a simulation stop time and execute the simulation run
scSim.ConfigureStopTime(simulationTime)
scSim.ExecuteSimulation()
# retrieve the logged data
attData = dataLog.sigma_BN
mtbData = dataLogMTB.mtbNetTorque_B
dataMagField = dataLogMag.magField_N
np.set_printoptions(precision=16)
# plot net mtb torque
if show_plots:
plt.close("all") # clears out plots from earlier test runs
plt.figure(1)
for idx in range(0, 3):
plt.plot(dataLogMTB.times() * macros.NANO2SEC, mtbData[:, idx],
color=unitTestSupport.getLineColor(idx, 3),
label=r'$\tau_' + str(idx) + '$')
plt.legend(loc='lower right')
plt.xlabel('Time [s]')
plt.ylabel(r'MTB Net Torque $\tau_{B}$ [Nm]')
# plot magnetic field data in the Inertial frame
plt.figure(2)
for idx in range(3):
plt.plot(dataLogMag.times() * macros.NANO2SEC, dataMagField[:, idx] * 1e9,
color=unitTestSupport.getLineColor(idx, 3),
label=r'$B\_N_{' + str(idx) + '}$')
plt.legend(loc='lower right')
plt.xlabel('Time [s]')
plt.ylabel('Magnetic Field [nT]')
# plot the Body relative to Inertial attitude
plt.figure(3)
for idx in range(0, 3):
plt.plot(dataLog.times() * macros.NANO2SEC, attData[:, idx],
color=unitTestSupport.getLineColor(idx, 3),
label=r'$\sigma_' + str(idx) + '$')
plt.legend(loc='lower right')
plt.xlabel('Time [s]')
plt.ylabel(r'MRP Attitude $\sigma_{B/N}$')
plt.show()
# compare gravity gradient torque vector to the truth
# use mtbData[1:, :] since mtbData is the torque from prev iterations
for sV, mtbTauV, bV in zip(attData[1:, :], mtbData[1:, :], dataMagField):
mtbTauTruth = truthMagneticTorque(bV, sV, mtbCmdInMsgContainer.mtbDipoleCmds,
mtbConfigParams.GtMatrix_B,
mtbConfigParams.numMTB,
maxDipole
)
testFailCount, testMessages = unitTestSupport.compareVector(mtbTauV,
mtbTauTruth,
accuracy,
"mtbTorque_B",
testFailCount, testMessages)
if testFailCount == 0:
print("PASSED: Mtb Effector")
else:
print("Failed: Mtb Effector")
return testFailCount, testMessages
if __name__ == "__main__":
test_MtbEffector(
False,
1e-12,
0.1
)