How do I get insight into/data from the dynamics integration steps?

[MichaelBonnet]

Hello,

I'm working on scenario where I use some external Basilisk modules as FSW to try and detumble a spacecraft with three magnetorquers and a magnetometer. This is on tag v2.5.0. See the script below:

import os
import matplotlib.pyplot as plt
import numpy as np
import pandas as pd
# The path to the location of Basilisk support data.
from Basilisk import __path__
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import tamComm, torque2Dipole, dipoleMapping
from Basilisk.simulation import simpleNav, magneticFieldWMM, magnetometer, MtbEffector, spacecraft
from Basilisk.utilities import SimulationBaseClass, macros, orbitalMotion, simIncludeGravBody, unitTestSupport
from Basilisk.ExternalModules import MagfieldEstimatorModule, MagRateDampModeModule, BdotControllerModule

bskPath = __path__[0]
fileName = os.path.basename(os.path.splitext(__file__)[0])

# ==============================================================================
# Plotting helper functions (same as before)
# ==============================================================================
def plot_magnetic_field(timeData, dataMagField):
    plt.figure(0)
    for idx in range(3):
        plt.plot(timeData, dataMagField[:, idx],
                 color=unitTestSupport.getLineColor(idx, 3),
                 label=r'$B\_N_{' + str(idx) + '}$')
    plt.legend(loc='lower right')
    plt.xlabel('Time [min]')
    plt.ylabel('Magnetic Field from WMM [T]')
    plt.title('Spacecraft Local Magnetic Field')
    plt.grid(True)

def plot_data_tam(timeData, dataTam):
    plt.figure(1)
    for idx in range(3):
        plt.plot(timeData, dataTam[:, idx],
                 color=unitTestSupport.getLineColor(idx, 3),
                 label=r'$TAM\_S_{' + str(idx) + '}$')
    plt.legend(loc='lower right')
    plt.xlabel('Time [min]')
    plt.ylabel('Magnetic Field from Magnetometer [T]')
    plt.grid(True)
    plt.title('Spacecraft Local Magnetic Field in Sensor Frame')
    
def plot_data_tam_comm(timeData, dataTamComm):
    plt.figure(2)
    for idx in range(3):
        plt.plot(timeData, dataTamComm[:, idx],
                 color=unitTestSupport.getLineColor(idx, 3),
                 label=r'$TAM\_B_{' + str(idx) + '}$')
    plt.legend(loc='lower right')
    plt.xlabel('Time [min]')
    plt.ylabel('Magnetic Field from Magnetometer in Body Frame [T]')
    plt.grid(True)
    plt.title('Spacecraft Local Magnetic Field in Body Frame')

def plot_data_angular_rates(timeData, dataScState_AngularRates):
    plt.figure(3)
    for idx in range(3):
        plt.plot(timeData, dataScState_AngularRates[:, idx],
                 color=unitTestSupport.getLineColor(idx, 3),
                 label=r'$omega\_B_{' + str(idx) + '}$')
    plt.legend(loc='lower right')
    plt.xlabel('Time [min]')
    plt.ylabel('Spacecraft Angular Rate [rad/s]')
    plt.grid(True)
    plt.title('Spacecraft Angular Body Rates')

def plot_bdot_torque_commands(timeData, dataBdotTorqueCmds):
    plt.figure(4)
    for idx in range(3):
        plt.plot(timeData, dataBdotTorqueCmds[:, idx],
                 color=unitTestSupport.getLineColor(idx, 3),
                 label=r'$Cmd\_Torque_{' + str(idx) + '}$')
    plt.legend(loc='lower right')
    plt.xlabel('Time [min]')
    plt.ylabel('BdotController Commanded Torque [Nm]')
    plt.grid(True)
    plt.title('BdotController Commanded Body Torques')

def plot_bdot_dipole_commands(timeData, dataBdotDipoleCmds):
    plt.figure(5)
    for idx in range(3):
        plt.plot(timeData, dataBdotDipoleCmds[:, idx],
                 color=unitTestSupport.getLineColor(idx, 3),
                 label=r'$Cmd\_dipole_{' + str(idx) + '}$')
    plt.legend(loc='lower right')
    plt.xlabel('Time [min]')
    plt.ylabel('BdotController Commanded Dipole [Am^2]')
    plt.grid(True)
    plt.title('BdotController Commanded Body Dipoles')

def plot_mtb_dipole_cmds(timeData, dataDipoleMappingRequests, numMTB):
    plt.figure(6)
    for idx in range(numMTB):
        plt.plot(timeData, dataDipoleMappingRequests[:, idx],
                 color=unitTestSupport.getLineColor(idx, numMTB),
                 label=r'$MTB\_T_{' + str(idx) + '}$')
    plt.legend(loc='lower right')
    plt.xlabel('Time [min]')
    plt.ylabel('dipoleMapping MTB Commands [Am^2]')
    plt.grid(True)
    plt.title('dipoleMapping Torque Rod Dipole Requests')

def plot_magfieldtlm_b_est_T(timeData, dataMagfieldEstimatorTlm_b_est_T):
    plt.figure(7)
    for idx in range(3):
        plt.plot(timeData, dataMagfieldEstimatorTlm_b_est_T[:, idx],
                 color=unitTestSupport.getLineColor(idx, 3),
                 label=r'$Est\_Bfield_{' + str(idx) + '}$')
    plt.legend(loc='lower right')
    plt.xlabel('Time [min]')
    plt.ylabel('Estimated Bfield [T]')
    plt.grid(True)
    plt.title("MagfieldEstimator Estimated Magnetic field")

def plot_mtbeffector_torque(timeData, dataMtbEff):
    plt.figure(8)
    for idx in range(3):
        plt.plot(timeData, dataMtbEff[:, idx],
                 color=unitTestSupport.getLineColor(idx, 3),
                 label=r'$MtbEffector\_Torque_{' + str(idx) + '}$')
    plt.legend(loc='lower right')
    plt.xlabel('Time [min]')
    plt.ylabel('MtbEffector Net Torque [Nm]')
    plt.grid(True)
    plt.title("MtbEffector Net Torque")


# ==============================================================================
# Main simulation run function
# ==============================================================================
def run(show_plots):
    """
    This simulation chain is ordered as:
      0. Spacecraft (scObject) writes its state.
      1. SimpleNav processes the state.
      2. MagneticFieldWMM uses the state to write an environmental magnetic field.
      3. Magnetometer (TAM) reads the magnetic field and writes a sensor message.
      4. tamComm reads the sensor message.
      5. MagfieldEstimatorModule processes tamComm output.
      6. MagRateDampModeModule uses the estimator output.
      7. BdotControllerModule computes a dipole command.
      8. dipoleMapping maps that command.
      9. MtbEffector reads the mapped command and applies a torque.
    
    To avoid “un-written message” errors, prime the TAM and BdotController outputs with dummy data immediately after initialization.
    """
    # Create simulation container, process, and task
    simTaskName = "simTask"
    simProcessName = "simProcess"
    scSim = SimulationBaseClass.SimBaseClass()
    simulationTime = macros.min2nano(285)
    dynProcess = scSim.CreateNewProcess(simProcessName)
    simulationTimeStep = macros.sec2nano(0.1)
    dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep))
    
    # -------------------------------
    # Spacecraft dynamics setup
    # -------------------------------
    scObject = spacecraft.Spacecraft()
    scObject.ModelTag = "basicSat"
    I = [2.91628e+1, 5.08581e-1, -3.0317801e-2,
         5.08581e-1, 1.71388e+1, 4.98592e-1,
         -3.0317801e-2, 4.98592e-1, 3.338064e+1]
    scObject.hub.mHub = 2.072172e+2
    scObject.hub.r_BcB_B = [[7.9974e-3], [1.3487e-2], [2.914512e-1]]
    scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I)
    # Add spacecraft first so its state is available.
    scSim.AddModelToTask(simTaskName, scObject, 0)
    
    # Gravity and orbital setup
    gravFactory = simIncludeGravBody.gravBodyFactory()
    earth = gravFactory.createEarth()
    earth.isCentralBody = True
    mu = earth.mu
    gravFactory.addBodiesTo(scObject)
    
    # -------------------------------
    # Navigation module
    # -------------------------------
    sNavObject = simpleNav.SimpleNav()
    sNavObject.ModelTag = "SimpleNavigation"
    scSim.AddModelToTask(simTaskName, sNavObject, 1)
    
    # -------------------------------
    # Environment and sensor modules
    # -------------------------------
    magneticFieldModelModule = magneticFieldWMM.MagneticFieldWMM()
    magneticFieldModelModule.ModelTag = "WMM"
    magneticFieldModelModule.dataPath = bskPath + '/supportData/MagneticField/'
    epochMsg = unitTestSupport.timeStringToGregorianUTCMsg('2024 April 20, 00:00:0.0 (UTC)')
    magneticFieldModelModule.epochInMsg.subscribeTo(epochMsg)
    magneticFieldModelModule.addSpacecraftToModel(scObject.scStateOutMsg)
    scSim.AddModelToTask(simTaskName, magneticFieldModelModule, 2)
    
    TAM = magnetometer.Magnetometer()
    TAM.ModelTag = "TAM_sensor"
    TAM.scaleFactor = 1.0
    TAM.senNoiseStd = [0.0, 0.0, 0.0]
    scSim.AddModelToTask(simTaskName, TAM, 3)
    
    tamCommObj = tamComm.tamComm()
    tamCommObj.dcm_BS = [1., 0., 0., 0., 1., 0., 0., 0., 1.]
    tamCommObj.ModelTag = "tamComm"
    scSim.AddModelToTask(simTaskName, tamCommObj, 4)
    
    # -------------------------------
    # FSW modules
    # -------------------------------
    magEstimator = MagfieldEstimatorModule.MagfieldEstimatorModule()
    magEstimator.ModelTag = "MagfieldEstimator"
    magEstimator.measureTime_ns = macros.sec2nano(1)
    magEstimator.actuateTime_ns = macros.sec2nano(9)
    magEstimator.executionRate_Hz = 10.0
    scSim.AddModelToTask(simTaskName, magEstimator, 5)
    
    magRateDamp = MagRateDampModeModule.MagRateDampModeModule()
    magRateDamp.ModelTag = "MagRateDampMode"
    scSim.AddModelToTask(simTaskName, magRateDamp, 6)
    
    bdotController = BdotControllerModule.BdotControllerModule()
    bdotController.ModelTag = "bDotController"
    bdotController.gain = 3
    scSim.AddModelToTask(simTaskName, bdotController, 7)
    
    dipoleMappingObj = dipoleMapping.dipoleMapping()
    dipoleMappingObj.ModelTag = "dipoleMapping"
    dipoleMappingObj.steeringMatrix = [1.0, 0.0, 0.0,
                                         0.0, -1.0, 0.0,
                                         -0.214286, 0.0, 1.02041]
    scSim.AddModelToTask(simTaskName, dipoleMappingObj, 8)
    
    # -------------------------------
    # Actuator module
    # -------------------------------
    mtbEff = MtbEffector.MtbEffector()
    mtbEff.ModelTag = "MtbEff"
    scObject.addDynamicEffector(mtbEff)
    scSim.AddModelToTask(simTaskName, mtbEff, 9)
    
    # -------------------------------
    # MTB configuration message
    # -------------------------------
    mtbConfigParams = messaging.MTBArrayConfigMsgPayload()
    mtbConfigParams.numMTB = 3
    mtbConfigParams.GtMatrix_B = [1.0, 0.0, 0.0,
                                  0.0, -1.0, 0.0,
                                  0.21, 0.0, 0.98]
    maxDipole = 3.04e+2
    mtbConfigParams.maxMtbDipoles = [maxDipole] * mtbConfigParams.numMTB
    mtbParamsInMsg = messaging.MTBArrayConfigMsg().write(mtbConfigParams)
    
    # -------------------------------
    # Connect messages along the chain
    # -------------------------------
    sNavObject.scStateInMsg.subscribeTo(scObject.scStateOutMsg)
    TAM.stateInMsg.subscribeTo(scObject.scStateOutMsg)
    TAM.magInMsg.subscribeTo(magneticFieldModelModule.envOutMsgs[0])
    tamCommObj.tamInMsg.subscribeTo(TAM.tamDataOutMsg)
    
    magEstimator.magSensorInMsg.subscribeTo(tamCommObj.tamOutMsg)
    magRateDamp.magfieldEstimatorInMsg.subscribeTo(magEstimator.magfieldEstimatorTlmOutMsg)
    bdotController.magRateDampModeInMsg.subscribeTo(magRateDamp.magRateDampModeOutMsg)
    dipoleMappingObj.dipoleRequestBodyInMsg.subscribeTo(bdotController.dipoleRequestBodyOutMsg)
    dipoleMappingObj.mtbArrayConfigParamsInMsg.subscribeTo(mtbParamsInMsg)
    mtbEff.mtbCmdInMsg.subscribeTo(dipoleMappingObj.dipoleRequestMtbOutMsg)
    mtbEff.mtbParamsInMsg.subscribeTo(mtbParamsInMsg)
    mtbEff.magInMsg.subscribeTo(magneticFieldModelModule.envOutMsgs[0])
    
    # -------------------------------
    # Data logging
    # -------------------------------
    numDataPoints = 171000
    samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints)
    
    magLog = magneticFieldModelModule.envOutMsgs[0].recorder(samplingTime)
    tamLog = TAM.tamDataOutMsg.recorder(samplingTime)
    tamCommLog = tamCommObj.tamOutMsg.recorder(samplingTime)
    magfieldEstimatorTlmLog = magEstimator.magfieldEstimatorTlmOutMsg.recorder(samplingTime)
    magRateDampTlmLog = magRateDamp.magRateDampModeOutMsg.recorder(samplingTime)
    bdotTorqueCmdsLog = bdotController.cmdTorqueBodyOutMsg.recorder(samplingTime)
    bdotDipoleCmdsLog = bdotController.dipoleRequestBodyOutMsg.recorder(samplingTime)
    bdotTlmLog = bdotController.bdotControllerTlmOutMsg.recorder(samplingTime)
    mtbDipoleRequestLog = dipoleMappingObj.dipoleRequestMtbOutMsg.recorder(samplingTime)
    scStateLog = scObject.scStateOutMsg.recorder(samplingTime)
    mtbOutLog = mtbEff.mtbOutMsg.recorder(samplingTime)
    
    scSim.AddModelToTask(simTaskName, magLog)
    scSim.AddModelToTask(simTaskName, tamLog)
    scSim.AddModelToTask(simTaskName, tamCommLog)
    scSim.AddModelToTask(simTaskName, magfieldEstimatorTlmLog)
    scSim.AddModelToTask(simTaskName, magRateDampTlmLog)
    scSim.AddModelToTask(simTaskName, bdotTorqueCmdsLog)
    scSim.AddModelToTask(simTaskName, bdotDipoleCmdsLog)
    scSim.AddModelToTask(simTaskName, bdotTlmLog)
    scSim.AddModelToTask(simTaskName, mtbDipoleRequestLog)
    scSim.AddModelToTask(simTaskName, scStateLog)
    scSim.AddModelToTask(simTaskName, mtbOutLog)
    
    # -------------------------------
    # Set initial spacecraft state via orbital elements
    # -------------------------------
    oe = orbitalMotion.ClassicElements()
    oe.a = (6378.0 + 550) * 1e3
    oe.e = 0.00
    oe.i = 98.0 * macros.D2R
    oe.Omega = 0.0 * macros.D2R
    oe.omega = 0.0 * macros.D2R
    oe.f = 0.0 * macros.D2R
    rN, vN = orbitalMotion.elem2rv(mu, oe)
    scObject.hub.r_CN_NInit = rN
    scObject.hub.v_CN_NInit = vN
    scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]]
    scObject.hub.omega_BN_BInit = [[0.1], [-0.1], [0.1]]
    
    # -------------------------------
    # Initialize and prime output messages
    # -------------------------------
    print("Initializing ...")
    scSim.InitializeSimulation()
    
    # Prime the TAM sensor output so its message is valid from time zero.
    dummyTAM = messaging.TAMSensorMsgPayload()
    dummyTAM.tam_S = [0.0, 0.0, 0.0]
    TAM.tamDataOutMsg.write(dummyTAM)
    
    # Prime the BdotController dipole output so its message is valid.
    dummyDipole = messaging.DipoleRequestBodyMsgPayload()
    dummyDipole.dipole_B = [0.0, 0.0, 0.0]
    bdotController.dipoleRequestBodyOutMsg.write(dummyDipole)
    
    scSim.ConfigureStopTime(simulationTime)
    print("Running ...")
    scSim.ExecuteSimulation()
    print("Simulation complete.")
    
    # -------------------------------
    # Retrieve and process logged data (post-processing code remains the same)
    # -------------------------------
    dataMagField = magLog.magField_N
    dataTam = tamLog.tam_S
    dataTamComm = tamCommLog.tam_B
    dataMagfieldEstimatorTlm_b_est_T = magfieldEstimatorTlmLog.b_est_T
    dataBdotTorqueCmds = bdotTorqueCmdsLog.torqueRequestBody
    dataBdotDipoleCmds = bdotDipoleCmdsLog.dipole_B
    dataBdotTlmTorqueCmds = bdotTlmLog.commanded_torque
    dataBdotTlmDipoleCmds = bdotTlmLog.commanded_dipole
    dataDipoleMappingRequests = mtbDipoleRequestLog.mtbDipoleCmds
    dataScState_AngularRates = scStateLog.omega_BN_B
    dataMtbEff = mtbOutLog.mtbNetTorque_B
    
    # (Optional) Create a DataFrame, CSV, etc.
    
    # Plot results
    plt.close("all")
    figureList = {}
    timeData = tamLog.times() * macros.NANO2MIN
    plot_magnetic_field(timeData, dataMagField)
    figureList[fileName + "1"] = plt.figure(0)
    plot_data_tam(timeData, dataTam)
    figureList[fileName + "2"] = plt.figure(1)
    plot_data_tam_comm(timeData, dataTamComm)
    figureList[fileName + "3"] = plt.figure(2)
    plot_data_angular_rates(timeData, dataScState_AngularRates)
    figureList[fileName + "4"] = plt.figure(3)
    plot_bdot_torque_commands(timeData, dataBdotTorqueCmds)
    figureList[fileName + "5"] = plt.figure(4)
    plot_bdot_dipole_commands(timeData, dataBdotDipoleCmds)
    figureList[fileName + "6"] = plt.figure(5)
    plot_mtb_dipole_cmds(timeData, dataDipoleMappingRequests, mtbConfigParams.numMTB)
    figureList[fileName + "7"] = plt.figure(6)
    plot_magfieldtlm_b_est_T(timeData, dataMagfieldEstimatorTlm_b_est_T)
    figureList[fileName + "8"] = plt.figure(7)
    plot_mtbeffector_torque(timeData, dataMtbEff)
    figureList[fileName + "9"] = plt.figure(8)
    
    if show_plots:
        plt.show()
    plt.close("all")
    return figureList


if __name__ == "__main__":
    run(True)

It produces plots like so:

As you can see, the commanded dipoles and torques are produced, and the commanded dipole is passed into dipoleMapping then on into the MtbEffector. MtbEffector reports large net torques that vary as espected with the commanded dipoles/torques. However, the spacecraft body angular rates never change - or rather, their trends are stable, as opposed to decreasing or even increasing.

I want to be able to better diagnose this problem, as I believe I have set everything up correctly. My steering matrix is the inverse of my torquer bar alignment matrix. Everything talks to each other, and were added to the simulation in well-defined priorities. I even tried reversing the priorities across the chain to see if that changed anything - nada. This leads me to suspect that the torques from MtbEffector are not finding their way into the dynamics integration step.

The issue is, a lot of the variables I'd interrogate to find data and help answer "what all is going into the dynamics integration" and hopefully "why are't these torques getting into dynamics integration" don't have associated messages I can log and then plot/analyze.

1. How do I get visibility into the inner workings of the dynamics integration steps in a given simulation
2. How do I check what values are getting used in dynamics integration?
3. Am I doing something wrong in how I've set up the simulation in general? My custom FSW uses Avanzini and Giulietti equations 15 and 4 to calculate commanded dipole and commanded torque, respectively.
4. Might there be a way to command individual magnetorquers "directly"?

Thanks for any and all help.