Source code for test_prescribedTrans

#
#  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:        prescribedTrans
#   Author:             Leah Kiner
#   Creation Date:      Jan 2, 2023
#

import inspect
import matplotlib.pyplot as plt
import numpy as np
import os
import pytest
from Basilisk.architecture import bskLogging
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import prescribedTrans  # import the module that is to be tested
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import macros
from Basilisk.utilities import unitTestSupport

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


# Vary the initial angle, reference angle, and maximum angular acceleration for pytest
[docs] @pytest.mark.parametrize("scalarPosInit", [0, 2*np.pi/3]) @pytest.mark.parametrize("scalarPosRef", [0, 2*np.pi/3]) @pytest.mark.parametrize("scalarAccelMax", [0.0005, 0.002]) @pytest.mark.parametrize("accuracy", [1e-12]) # update "module" in this function name to reflect the module name def test_prescribedTransTestFunction(show_plots, scalarPosInit, scalarPosRef, scalarAccelMax, accuracy): r""" **Validation Test Description** This unit test ensures that the profiled translation for a secondary prescribed rigid body connected to the spacecraft hub 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. **Test Parameters** Args: 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 translation 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. """ [testResults, testMessage] = prescribedTransTestFunction(show_plots, scalarPosInit, scalarPosRef, scalarAccelMax, accuracy) assert testResults < 1, testMessage
[docs] def prescribedTransTestFunction(show_plots, scalarPosInit, scalarPosRef, scalarAccelMax, accuracy): """Call this routine directly to run the unit test.""" testFailCount = 0 testMessages = [] unitTaskName = "unitTask" unitProcessName = "TestProcess" bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread testProcessRate = macros.sec2nano(0.1) testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm and associated C++ container PrescribedTrans = prescribedTrans.prescribedTrans() PrescribedTrans.ModelTag = "prescribedTrans" # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, PrescribedTrans) # Initialize the test module configuration data transAxis_M = np.array([0.5, 0.0, 0.5 * np.sqrt(3)]) PrescribedTrans.transAxis_M = transAxis_M PrescribedTrans.scalarAccelMax = scalarAccelMax # [rad/s^2] PrescribedTrans.r_FM_M = scalarPosInit * transAxis_M PrescribedTrans.rPrime_FM_M = np.array([0.0, 0.0, 0.0]) PrescribedTrans.rPrimePrime_FM_M = np.array([0.0, 0.0, 0.0]) # Create input message scalarVelRef = 0.0 # [m/s] linearTranslationRigidBodyMessageData = messaging.LinearTranslationRigidBodyMsgPayload() linearTranslationRigidBodyMessageData.rho = scalarPosRef linearTranslationRigidBodyMessageData.rhoDot = scalarVelRef linearTranslationRigidBodyMessage = messaging.LinearTranslationRigidBodyMsg().write(linearTranslationRigidBodyMessageData) PrescribedTrans.linearTranslationRigidBodyInMsg.subscribeTo(linearTranslationRigidBodyMessage) # Setup logging on the test module output message so that we get all the writes to it dataLog = PrescribedTrans.prescribedTranslationOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) # Need to call the self-init and cross-init methods unitTestSim.InitializeSimulation() # Set the simulation time simTime = np.sqrt(((0.5 * np.abs(scalarPosRef - scalarPosInit)) * 8) / scalarAccelMax) + 5 unitTestSim.ConfigureStopTime(macros.sec2nano(simTime)) # Begin the simulation time run set above unitTestSim.ExecuteSimulation() # Extract logged data r_FM_M = dataLog.r_FM_M rPrime_FM_M = dataLog.rPrime_FM_M timespan = dataLog.times() scalarVel_Final = np.linalg.norm(rPrime_FM_M[-1, :]) scalarPos_Final = np.linalg.norm(r_FM_M[-1, :]) # Plot r_FM_F r_FM_M_Ref = scalarPosRef * 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='$r\'_{1}$') plt.plot(timespan * macros.NANO2SEC, rPrime_FM_M[:, 1], label='$r\'_{2}$') plt.plot(timespan * macros.NANO2SEC, rPrime_FM_M[:, 2], label='$r\'_{3}$') plt.title(r'${}^\mathcal{M} r\'_{\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}) if show_plots: plt.show() plt.close("all") # set the filtered output truth states if not unitTestSupport.isDoubleEqual(scalarVel_Final, scalarVelRef, accuracy): testFailCount += 1 testMessages.append("FAILED: " + PrescribedTrans.ModelTag + "scalarVel_Final and scalarVelRef do not match") if not unitTestSupport.isDoubleEqual(scalarPos_Final, scalarPosRef, accuracy): testFailCount += 1 testMessages.append("FAILED: " + PrescribedTrans.ModelTag + "scalarPos_Final and scalarPosRef do not match") return [testFailCount, ''.join(testMessages)]
# # This statement below ensures that the unitTestScript can be run as a # stand-along python script # if __name__ == "__main__": prescribedTransTestFunction( True, 0.0, # scalarPosInit 0.25, # scalarPosRef 0.001, # scalarAccelMax 1e-12 # accuracy )