Source code for test_PrescribedMotionStateEffector

# ISC License
#
# Copyright (c) 2023, 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.


#
#   Unit Test Script
#   Module Name:        prescribedMotion integrated unit test with prescribedRotation1DOF and prescribedLinearTranslation
#   Author:             Leah Kiner
#   Creation Date:      Jan 10, 2022
#

import inspect
import os

import matplotlib
import matplotlib.pyplot as plt
import numpy as np
import pytest

from Basilisk.architecture import messaging
from Basilisk.simulation import gravityEffector
from Basilisk.simulation import prescribedLinearTranslation
from Basilisk.simulation import prescribedMotionStateEffector
from Basilisk.simulation import prescribedRotation1DOF
from Basilisk.simulation import spacecraft
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import macros
from Basilisk.utilities import RigidBodyKinematics as rbk
from Basilisk.utilities import unitTestSupport

filename = inspect.getframeinfo(inspect.currentframe()).filename
path = os.path.dirname(os.path.abspath(filename))
splitPath = path.split('simulation')

matplotlib.rc('xtick', labelsize=16)
matplotlib.rc('ytick', labelsize=16)

# Vary the simulation parameters for pytest
[docs] @pytest.mark.parametrize("rotTest", [True, False]) @pytest.mark.parametrize("thetaInit", [0, np.pi/18]) @pytest.mark.parametrize("theta_Ref", [np.pi/36]) @pytest.mark.parametrize("posInit", [0, 0.2]) @pytest.mark.parametrize("posRef", [0.1]) @pytest.mark.parametrize("accuracy", [1e-8]) def test_PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posInit, posRef, accuracy): r""" **Validation Test Description** The unit test for this module is an integrated test with two flight software profiler modules. This is required because the dynamics module must be connected to a flight software profiler module to define the states of the prescribed secondary body that is connected to the rigid spacecraft hub. The integrated test for this module has two simple scenarios it is testing. The first scenario prescribes a 1 DOF rotation for the prescribed body using the :ref:`prescribedRotation1DOF` flight software module. The second scenario prescribes linear translation for the prescribed body using the :ref:`prescribedLinearTranslation` flight software module. This unit test ensures that the profiled 1 DOF rotational attitude maneuver is properly computed for a series of initial and reference PRV angles and maximum angular accelerations. The final prescribed attitude and angular velocity magnitude are compared with the reference values. This unit test also ensures that the profiled translational maneuver is properly computed for a series of initial and reference positions and maximum accelerations. The final prescribed position and velocity magnitudes are compared with the reference values. Additionally for each scenario, the conservation quantities of orbital angular momentum, rotational angular momentum, and orbital energy are checked to validate the module dynamics. **Test Parameters** Args: rotTest (bool): (True) Runs the rotational motion test. (False) Runs the translational motion test. thetaInit (float): [rad] Initial PRV angle of the F frame with respect to the M frame theta_Ref (float): [rad] Reference PRV angle of the F frame with respect to the M frame thetaDDotMax (float): [rad/s^2] Maximum angular acceleration for the attitude maneuver scalarPosInit (float): [m] Initial scalar position of the F frame with respect to the M frame scalarPosRef (float): [m] Reference scalar position of the F frame with respect to the M frame scalarAccelMax (float): [m/s^2] Maximum acceleration for the translational maneuver accuracy (float): absolute accuracy value used in the validation tests **Description of Variables Being Tested** This unit test ensures that the profiled 1 DOF rotational attitude maneuver is properly computed for a series of initial and reference PRV angles and maximum angular accelerations. The final prescribed angle ``theta_FM_Final`` and angular velocity magnitude ``thetaDot_Final`` are compared with the reference values ``theta_Ref`` and ``thetaDot_Ref``, respectively. This unit test also ensures that the profiled translational maneuver is properly computed for a series of initial and reference positions and maximum accelerations. The final prescribed position magnitude ``r_FM_M_Final`` and velocity magnitude ``rPrime_FM_M_Final`` are compared with the reference values ``r_FM_M_Ref`` and ``rPrime_FM_M_Ref``, respectively. Additionally for each scenario, the conservation quantities of orbital angular momentum, rotational angular momentum, and orbital energy are checked to validate the module dynamics. """ [testResults, testMessage] = PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posInit, posRef, accuracy) assert testResults < 1, testMessage
[docs] def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posInit, posRef, accuracy): """Call this routine directly to run the unit test.""" __tracebackhide__ = True testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages unitTaskName = "unitTask" unitProcessName = "TestProcess" # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread testIncrement = 0.1 # [s] testProcessRate = macros.sec2nano(testIncrement) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Add the spacecraft module to test file scObject = spacecraft.Spacecraft() scObject.ModelTag = "spacecraftBody" # Define the mass properties of the rigid spacecraft hub scObject.hub.mHub = 750.0 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] # Set the initial inertial hub states scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] scObject.hub.omega_BN_BInit = np.array([0.0, 0.0, 0.0]) scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] # Add the scObject to the runtime call list unitTestSim.AddModelToTask(unitTaskName, scObject) # Add the prescribedMotion dynamics module to test file platform = prescribedMotionStateEffector.PrescribedMotionStateEffector() # Define the state effector properties transAxis_M = np.array([1.0, 0.0, 0.0]) rotAxis_M = np.array([1.0, 0.0, 0.0]) r_FM_M = posInit * transAxis_M prvInit_FM = thetaInit * rotAxis_M sigma_FM = rbk.PRV2MRP(prvInit_FM) platform.mass = 100.0 platform.IPntFc_F = [[50.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]] platform.r_MB_B = [0.0, 0.0, 0.0] platform.r_FcF_F = [0.0, 0.0, 0.0] platform.r_FM_M = r_FM_M platform.rPrime_FM_M = np.array([0.0, 0.0, 0.0]) platform.rPrimePrime_FM_M = np.array([0.0, 0.0, 0.0]) platform.omega_FM_F = np.array([0.0, 0.0, 0.0]) platform.omegaPrime_FM_F = np.array([0.0, 0.0, 0.0]) platform.sigma_FM = sigma_FM platform.omega_MB_B = [0.0, 0.0, 0.0] platform.omegaPrime_MB_B = [0.0, 0.0, 0.0] platform.sigma_MB = [0.0, 0.0, 0.0] platform.ModelTag = "Platform" # Add platform to spacecraft scObject.addStateEffector(platform) # Add the test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, platform) if rotTest: # ** ** ** ** ** ROTATIONAL 1 DOF INTEGRATED TEST: ** ** ** ** ** # Create an instance of the prescribedRotation1DOF module to be tested PrescribedRot1DOF = prescribedRotation1DOF.PrescribedRotation1DOF() PrescribedRot1DOF.ModelTag = "prescribedRotation1DOF" # Add the prescribedRotation1DOF test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, PrescribedRot1DOF) # Initialize the prescribedRotation1DOF test module configuration data accelMax = 0.01 # [rad/s^2] PrescribedRot1DOF.setRotHat_M(rotAxis_M) PrescribedRot1DOF.setThetaDDotMax(accelMax) PrescribedRot1DOF.setThetaInit(thetaInit) # Create the prescribedRotation1DOF input message thetaDot_Ref = 0.0 # [rad/s] SpinningBodyMessageData = messaging.HingedRigidBodyMsgPayload() SpinningBodyMessageData.theta = theta_Ref SpinningBodyMessageData.thetaDot = thetaDot_Ref SpinningBodyMessage = messaging.HingedRigidBodyMsg().write(SpinningBodyMessageData) PrescribedRot1DOF.spinningBodyInMsg.subscribeTo(SpinningBodyMessage) platform.prescribedRotationInMsg.subscribeTo(PrescribedRot1DOF.prescribedRotationOutMsg) # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" earthGravBody.mu = 0.3986004415E+15 earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) # Add energy and momentum variables to log scObjectLog = scObject.logger(["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"]) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Add other states to log scStateData = scObject.scStateOutMsg.recorder() prescribedRotStateData = platform.prescribedRotationOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, scStateData) unitTestSim.AddModelToTask(unitTaskName, prescribedRotStateData) # Initialize the simulation unitTestSim.InitializeSimulation() # Set the simulation time simTime = np.sqrt(((0.5 * np.abs(theta_Ref - thetaInit)) * 8) / accelMax) + 3 * testIncrement unitTestSim.ConfigureStopTime(macros.sec2nano(simTime)) # Begin the simulation unitTestSim.ExecuteSimulation() # Extract the logged data orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) omega_BN_B = scStateData.omega_BN_B r_BN_N = scStateData.r_BN_N sigma_BN = scStateData.sigma_BN omega_FM_F = prescribedRotStateData.omega_FM_F omegaPrime_FM_F = prescribedRotStateData.omegaPrime_FM_F sigma_FM = prescribedRotStateData.sigma_FM timespan = prescribedRotStateData.times() thetaDot_Final = np.linalg.norm(omega_FM_F[-1, :]) sigma_FM_Final = sigma_FM[-1, :] theta_FM_Final = 4 * np.arctan(np.linalg.norm(sigma_FM_Final)) # Setup the conservation quantities initialOrbAngMom_N = [[orbAngMom_N[0, 1], orbAngMom_N[0, 2], orbAngMom_N[0, 3]]] finalOrbAngMom = [orbAngMom_N[-1]] initialRotAngMom_N = [[rotAngMom_N[0, 1], rotAngMom_N[0, 2], rotAngMom_N[0, 3]]] finalRotAngMom = [rotAngMom_N[-1]] initialOrbEnergy = [[orbEnergy[0, 1]]] finalOrbEnergy = [orbEnergy[-1]] # Convert the logged sigma_FM MRPs to a scalar theta_FM array n = len(timespan) theta_FM = [] for i in range(n): theta_FM.append((180 / np.pi) * (4 * np.arctan(np.linalg.norm(sigma_FM[i, :])))) plt.close("all") # Plot theta_FM theta_Ref_plotting = np.ones(len(timespan)) * theta_Ref thetaInit_plotting = np.ones(len(timespan)) * thetaInit plt.figure() plt.clf() plt.plot(timespan * macros.NANO2SEC, theta_FM, label=r'$\Phi$') plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * theta_Ref_plotting, '--', label=r'$\Phi_{Ref}$') plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * thetaInit_plotting, '--', label=r'$\Phi_{0}$') plt.title(r'$\Phi_{\mathcal{F}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) plt.ylabel('(deg)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.legend(loc='center right', prop={'size': 16}) # Plot omega_FM_F plt.figure() plt.clf() plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_FM_F[:, 0], label=r'$\omega_{1}$') plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_FM_F[:, 1], label=r'$\omega_{2}$') plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_FM_F[:, 2], label=r'$\omega_{3}$') plt.title(r'${}^\mathcal{F} \omega_{\mathcal{F}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) plt.ylabel('(deg/s)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.legend(loc='upper right', prop={'size': 16}) # Plotting omegaPrime_FM_F plt.figure() plt.clf() plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omegaPrime_FM_F[:, 0], label='1') plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omegaPrime_FM_F[:, 1], label='2') plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omegaPrime_FM_F[:, 2], label='3') plt.title(r'${}^\mathcal{F} \omega Prime_{\mathcal{F}/\mathcal{M}}$ Profiled Angular Acceleration', fontsize=14) plt.ylabel(r'(deg/$s^2$)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.legend(loc='upper right', prop={'size': 16}) # Plot r_BN_N plt.figure() plt.clf() plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 0], label=r'$r_{1}$') plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 1], label=r'$r_{2}$') plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 2], label=r'$r_{3}$') plt.title(r'${}^\mathcal{N} r_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial Trajectory', fontsize=14) plt.ylabel('(m)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.legend(loc='center left', prop={'size': 16}) # Plot sigma_BN plt.figure() plt.clf() plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 0], label=r'$\sigma_{1}$') plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 1], label=r'$\sigma_{2}$') plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 2], label=r'$\sigma_{3}$') plt.title(r'$\sigma_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial MRP Attitude', fontsize=14) plt.ylabel('', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.legend(loc='center left', prop={'size': 16}) # Plot omega_BN_B plt.figure() plt.clf() plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 0], label=r'$\omega_{1}$') plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 1], label=r'$\omega_{2}$') plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 2], label=r'$\omega_{3}$') plt.title(r'Spacecraft Hub Angular Velocity ${}^\mathcal{B} \omega_{\mathcal{B}/\mathcal{N}}$', fontsize=14) plt.xlabel('Time (s)', fontsize=16) plt.ylabel('(deg/s)', fontsize=16) plt.legend(loc='lower right', prop={'size': 16}) # Plotting: Conservation quantities plt.figure() plt.clf() plt.plot(orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3]) plt.title('Orbital Angular Momentum Relative Difference', fontsize=14) plt.ylabel('(Nms)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.figure() plt.clf() plt.plot(orbEnergy[:, 0] * macros.NANO2SEC, (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1]) plt.title('Orbital Energy Relative Difference', fontsize=14) plt.ylabel('Energy (J)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.figure() plt.clf() plt.plot(rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]), rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]), rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 3] - rotAngMom_N[0, 3])) plt.title('Rotational Angular Momentum Difference', fontsize=14) plt.ylabel('(Nms)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.figure() plt.clf() plt.plot(rotEnergy[:, 0] * macros.NANO2SEC, (rotEnergy[:, 1] - rotEnergy[0, 1])) plt.title('Total Energy Difference', fontsize=14) plt.ylabel('Energy (J)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) if show_plots: plt.show() plt.close("all") # Begin the test analysis finalOrbAngMom = np.delete(finalOrbAngMom, 0, axis=1) # remove the time column finalRotAngMom = np.delete(finalRotAngMom, 0, axis=1) # remove the time column finalOrbEnergy = np.delete(finalOrbEnergy, 0, axis=1) # remove the time column for i in range(0, len(initialOrbAngMom_N)): if not unitTestSupport.isArrayEqualRelative(finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy): testFailCount += 1 testMessages.append( "FAILED: Prescribed Motion integrated test failed orbital angular momentum unit test") for i in range(0, len(initialRotAngMom_N)): if not unitTestSupport.isArrayEqual(finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy): testFailCount += 1 testMessages.append( "FAILED: Prescribed Motion integrated test failed rotational angular momentum unit test") for i in range(0, len(initialOrbEnergy)): if not unitTestSupport.isArrayEqualRelative(finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy): testFailCount += 1 testMessages.append("FAILED: Prescribed Motion integrated test failed orbital energy unit test") # Check to ensure the initial angle rate converged to the reference angle rate if not unitTestSupport.isDoubleEqual(thetaDot_Final, thetaDot_Ref, accuracy): testFailCount += 1 testMessages.append("FAILED: " + PrescribedRot1DOF.ModelTag + "thetaDot_Final and thetaDot_Ref do not match") # Check to ensure the initial angle converged to the reference angle if not unitTestSupport.isDoubleEqual(theta_FM_Final, theta_Ref, accuracy): testFailCount += 1 testMessages.append("FAILED: " + PrescribedRot1DOF.ModelTag + "theta_FM_Final and theta_Ref do not match") # testMessages.append("theta_FM_Final: " + str(theta_FM_Final) + " theta_Ref: " + str(theta_Ref)) if testFailCount == 0: print("PASSED: " + "prescribedMotion and prescribedRot1DOF integrated test") else: # ** ** ** ** ** TRANSLATIONAL INTEGRATED TEST ** ** ** ** ** # Create an instance of the prescribedLinearTranslation module to be tested PrescribedTrans = prescribedLinearTranslation.PrescribedLinearTranslation() PrescribedTrans.ModelTag = "prescribedLinearTranslation" # Add the prescribedLinearTranslation test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, PrescribedTrans) # Initialize the prescribedLinearTranslation test module configuration data accelMax = 0.005 # [m/s^2] PrescribedTrans.setTransHat_M(transAxis_M) PrescribedTrans.setTransAccelMax(accelMax) PrescribedTrans.setTransPosInit(posInit) # Create the prescribedTrans input message velRef = 0.0 # [m/s] linearTranslationRigidBodyMessageData = messaging.LinearTranslationRigidBodyMsgPayload() linearTranslationRigidBodyMessageData.rho = posRef linearTranslationRigidBodyMessageData.rhoDot = velRef linearTranslationRigidBodyMessage = messaging.LinearTranslationRigidBodyMsg().write(linearTranslationRigidBodyMessageData) PrescribedTrans.linearTranslationRigidBodyInMsg.subscribeTo(linearTranslationRigidBodyMessage) platform.prescribedTranslationInMsg.subscribeTo(PrescribedTrans.prescribedTranslationOutMsg) # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" earthGravBody.mu = 0.3986004415E+15 earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) # Add energy and momentum variables to log scObjectLog = scObject.logger(["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"]) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Add other states to log scStateData = scObject.scStateOutMsg.recorder() prescribedTransStateData = platform.prescribedTranslationOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, scStateData) unitTestSim.AddModelToTask(unitTaskName, prescribedTransStateData) # Initialize the simulation unitTestSim.InitializeSimulation() # Set the simulation time simTime = np.sqrt(((0.5 * np.abs(posRef - posInit)) * 8) / accelMax) + 3 * testIncrement unitTestSim.ConfigureStopTime(macros.sec2nano(simTime)) # Begin the simulation unitTestSim.ExecuteSimulation() # Extract the logged data orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) r_BN_N = scStateData.r_BN_N sigma_BN = scStateData.sigma_BN omega_BN_B = scStateData.omega_BN_B r_FM_M = prescribedTransStateData.r_FM_M rPrime_FM_M = prescribedTransStateData.rPrime_FM_M rPrimePrime_FM_M = prescribedTransStateData.rPrimePrime_FM_M timespan = prescribedTransStateData.times() r_FM_M_Final = r_FM_M[-1, :] rPrime_FM_M_Final = rPrime_FM_M[-1, :] # Setup the conservation quantities initialOrbAngMom_N = [[orbAngMom_N[0, 1], orbAngMom_N[0, 2], orbAngMom_N[0, 3]]] finalOrbAngMom = [orbAngMom_N[-1]] initialRotAngMom_N = [[rotAngMom_N[0, 1], rotAngMom_N[0, 2], rotAngMom_N[0, 3]]] finalRotAngMom = [rotAngMom_N[-1]] initialOrbEnergy = [[orbEnergy[0, 1]]] finalOrbEnergy = [orbEnergy[-1]] initialRotEnergy = [[rotEnergy[0, 1]]] finalRotEnergy = [rotEnergy[-1]] # Plot r_FM_F r_FM_M_Ref = posRef * transAxis_M r_FM_M_1_Ref = np.ones(len(timespan)) * r_FM_M_Ref[0] r_FM_M_2_Ref = np.ones(len(timespan)) * r_FM_M_Ref[1] r_FM_M_3_Ref = np.ones(len(timespan)) * r_FM_M_Ref[2] plt.figure() plt.clf() plt.plot(timespan * macros.NANO2SEC, r_FM_M[:, 0], label=r'$r_{1}$') plt.plot(timespan * macros.NANO2SEC, r_FM_M[:, 1], label=r'$r_{2}$') plt.plot(timespan * macros.NANO2SEC, r_FM_M[:, 2], label=r'$r_{3}$') plt.plot(timespan * macros.NANO2SEC, r_FM_M_1_Ref, '--', label=r'$r_{1 Ref}$') plt.plot(timespan * macros.NANO2SEC, r_FM_M_2_Ref, '--', label=r'$r_{2 Ref}$') plt.plot(timespan * macros.NANO2SEC, r_FM_M_3_Ref, '--', label=r'$r_{3 Ref}$') plt.title(r'${}^\mathcal{M} r_{\mathcal{F}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) plt.ylabel('(m)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.legend(loc='center left', prop={'size': 16}) # Plot rPrime_FM_F plt.figure() plt.clf() plt.plot(timespan * macros.NANO2SEC, rPrime_FM_M[:, 0], label='1') plt.plot(timespan * macros.NANO2SEC, rPrime_FM_M[:, 1], label='2') plt.plot(timespan * macros.NANO2SEC, rPrime_FM_M[:, 2], label='3') plt.title(r'${}^\mathcal{M} rPrime_{\mathcal{F}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) plt.ylabel('(m/s)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.legend(loc='upper left', prop={'size': 16}) # Plotting rPrimePrime_FM_M plt.figure() plt.clf() plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * rPrimePrime_FM_M[:, 0], label='1') plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * rPrimePrime_FM_M[:, 1], label='2') plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * rPrimePrime_FM_M[:, 2], label='3') plt.title(r'${}^\mathcal{M} rPrimePrime_{\mathcal{F}/\mathcal{M}}$ Profiled Acceleration', fontsize=14) plt.ylabel(r'(m/s$^2$)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.legend(loc='lower left', prop={'size': 16}) # Plot r_BN_N plt.figure() plt.clf() plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 0], label=r'$r_{1}$') plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 1], label=r'$r_{2}$') plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 2], label=r'$r_{3}$') plt.title(r'${}^\mathcal{N} r_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial Trajectory', fontsize=14) plt.ylabel('(m)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.legend(loc='center left', prop={'size': 16}) # Plot sigma_BN plt.figure() plt.clf() plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 0], label=r'$\sigma_{1}$') plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 1], label=r'$\sigma_{2}$') plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 2], label=r'$\sigma_{3}$') plt.title(r'$\sigma_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial MRP Attitude', fontsize=14) plt.ylabel('', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.legend(loc='lower left', prop={'size': 16}) # Plot omega_BN_B plt.figure() plt.clf() plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 0], label=r'$\omega_{1}$') plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 1], label=r'$\omega_{2}$') plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 2], label=r'$\omega_{3}$') plt.title(r'Spacecraft Hub Angular Velocity ${}^\mathcal{B} \omega_{\mathcal{B}/\mathcal{N}}$', fontsize=14) plt.ylabel('(deg/s)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.legend(loc='lower left', prop={'size': 16}) # Plotting: Conservation quantities plt.figure() plt.clf() plt.plot(orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3]) plt.title('Orbital Angular Momentum Relative Difference', fontsize=14) plt.ylabel('(Nms)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.figure() plt.clf() plt.plot(orbEnergy[:, 0] * macros.NANO2SEC, (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1]) plt.title('Orbital Energy Relative Difference', fontsize=14) plt.ylabel('Energy (J)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.figure() plt.clf() plt.plot(rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]), rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]), rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 3] - rotAngMom_N[0, 3])) plt.title('Rotational Angular Momentum Difference', fontsize=14) plt.ylabel('(Nms)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) plt.figure() plt.clf() plt.plot(rotEnergy[:, 0] * macros.NANO2SEC, (rotEnergy[:, 1] - rotEnergy[0, 1])) plt.title('Total Energy Difference', fontsize=14) plt.ylabel('Energy (J)', fontsize=16) plt.xlabel('Time (s)', fontsize=16) if show_plots: plt.show() plt.close("all") # Begin the test analysis finalOrbAngMom = np.delete(finalOrbAngMom, 0, axis=1) # remove the time column finalRotAngMom = np.delete(finalRotAngMom, 0, axis=1) # remove the time column finalOrbEnergy = np.delete(finalOrbEnergy, 0, axis=1) # remove the time column for i in range(0, len(initialOrbAngMom_N)): if not unitTestSupport.isArrayEqualRelative(finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy): testFailCount += 1 testMessages.append( "FAILED: Prescribed Motion integrated test failed orbital angular momentum unit test") for i in range(0, len(initialRotAngMom_N)): if not unitTestSupport.isArrayEqual(finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy): testFailCount += 1 testMessages.append( "FAILED: Prescribed Motion integrated test failed rotational angular momentum unit test") for i in range(0, len(initialOrbEnergy)): if not unitTestSupport.isArrayEqualRelative(finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy): testFailCount += 1 testMessages.append("FAILED: Prescribed Motion integrated test failed orbital energy unit test") # Check to ensure the initial velocity converged to the reference velocity rPrime_FM_M_Ref = np.array([0.0, 0.0, 0.0]) if not unitTestSupport.isArrayEqual(rPrime_FM_M_Final, rPrime_FM_M_Ref, 3, accuracy): testFailCount += 1 testMessages.append("FAILED: " + PrescribedTrans.ModelTag + "rPrime_FM_M_Final and rPrime_FM_M_Ref do not match") testMessages.append("rPrime_FM_M_Final: " + str(rPrime_FM_M_Final) + " rPrime_FM_M_Ref: " + str(rPrime_FM_M_Ref)) # Check to ensure the initial position converged to the reference position r_FM_M_Ref = np.array([posRef, 0.0, 0.0]) if not unitTestSupport.isArrayEqual(r_FM_M_Final, r_FM_M_Ref, 3, accuracy): testFailCount += 1 testMessages.append("FAILED: " + PrescribedTrans.ModelTag + "r_FM_M_Final and r_FM_M_Ref do not match") testMessages.append("r_FM_M_Final: " + str(r_FM_M_Final) + " r_FM_M_Ref: " + str(r_FM_M_Ref)) if testFailCount == 0: print("PASSED: " + "prescribedMotion and prescribedLinearTranslation integrated test") return [testFailCount, ''.join(testMessages)]
# # This statement below ensures that the unitTestScript can be run as a # stand-along python script # if __name__ == "__main__": PrescribedMotionTestFunction( True, # show_plots False, # rotTest, (True: prescribedRot1DOF integrated test, # False: prescribedTrans integrated test) 0.0, # thetaInit [rad] np.pi / 12, # theta_Ref [rad] 0.0, # posInit [m] 0.5, # posRef [m] 1e-8 # accuracy )