Source code for test_MtbEffector

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