Source code for test_prescribedRot1DOF

#
#  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:        prescribedRot1DOF
#   Author:             Leah Kiner
#   Creation Date:      Nov 14, 2022
#

import pytest
import inspect
import matplotlib.pyplot as plt
import numpy as np
import os
from Basilisk.architecture import bskLogging
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import prescribedRot1DOF  # import the module that is to be tested
from Basilisk.utilities import RigidBodyKinematics as rbk
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("thetaInit", [0, 2*np.pi/3]) @pytest.mark.parametrize("thetaRef", [0, 2*np.pi/3]) @pytest.mark.parametrize("thetaDDotMax", [0.008, 0.1]) @pytest.mark.parametrize("accuracy", [1e-12]) def test_prescribedRot1DOFTestFunction(show_plots, thetaInit, thetaRef, thetaDDotMax, accuracy): r""" **Validation Test Description** This unit test ensures that the profiled 1 DOF rotation for a secondary rigid body connected to the spacecraft hub 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. **Test Parameters** Args: thetaInit (float): [rad] Initial PRV angle of the F frame with respect to the M frame thetaRef (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 accuracy (float): absolute accuracy value used in the validation tests **Description of Variables Being Tested** This unit test ensures that the profiled 1 DOF rotation 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. """ [testResults, testMessage] = prescribedRot1DOFTestFunction(show_plots, thetaInit, thetaRef, thetaDDotMax, accuracy) assert testResults < 1, testMessage
[docs] def prescribedRot1DOFTestFunction(show_plots, thetaInit, thetaRef, thetaDDotMax, 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 the test thread testProcessRate = macros.sec2nano(0.1) testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Create an instance of the prescribedRot1DOF module to be tested PrescribedRot1DOF = prescribedRot1DOF.prescribedRot1DOF() PrescribedRot1DOF.ModelTag = "prescribedRot1DOF" # Add the prescribedRot1DOF test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, PrescribedRot1DOF) # Initialize the prescribedRot1DOF test module configuration data rotAxisM = np.array([1.0, 0.0, 0.0]) prvInit_FM = thetaInit * rotAxisM PrescribedRot1DOF.rotAxis_M = rotAxisM PrescribedRot1DOF.thetaDDotMax = thetaDDotMax PrescribedRot1DOF.omega_FM_F = np.array([0.0, 0.0, 0.0]) PrescribedRot1DOF.omegaPrime_FM_F = np.array([0.0, 0.0, 0.0]) PrescribedRot1DOF.sigma_FM = rbk.PRV2MRP(prvInit_FM) # Create the prescribedRot1DOF input message thetaDotRef = 0.0 # [rad/s] HingedRigidBodyMessageData = messaging.HingedRigidBodyMsgPayload() HingedRigidBodyMessageData.theta = thetaRef HingedRigidBodyMessageData.thetaDot = thetaDotRef HingedRigidBodyMessage = messaging.HingedRigidBodyMsg().write(HingedRigidBodyMessageData) PrescribedRot1DOF.spinningBodyInMsg.subscribeTo(HingedRigidBodyMessage) # Log the test module output message for data comparison dataLog = PrescribedRot1DOF.prescribedRotationOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) # Initialize the simulation unitTestSim.InitializeSimulation() # Set the simulation time simTime = np.sqrt(((0.5 * np.abs(thetaRef - thetaInit)) * 8) / thetaDDotMax) + 1 unitTestSim.ConfigureStopTime(macros.sec2nano(simTime)) # Begin the simulation unitTestSim.ExecuteSimulation() # Extract the logged data for plotting and data comparison omega_FM_F = dataLog.omega_FM_F sigma_FM = dataLog.sigma_FM timespan = dataLog.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)) # 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(4 * np.arctan(np.linalg.norm(sigma_FM[i, :]))) # Plot theta_FM thetaRef_plotting = np.ones(len(timespan)) * thetaRef 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) * thetaRef_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}) if show_plots: plt.show() plt.close("all") # Check to ensure the initial angle rate converged to the reference angle rate if not unitTestSupport.isDoubleEqual(thetaDot_Final, thetaDotRef, accuracy): testFailCount += 1 testMessages.append("FAILED: " + PrescribedRot1DOF.ModelTag + "thetaDot_Final and thetaDotRef do not match") # Check to ensure the initial angle converged to the reference angle if not unitTestSupport.isDoubleEqual(theta_FM_Final, thetaRef, accuracy): testFailCount += 1 testMessages.append("FAILED: " + PrescribedRot1DOF.ModelTag + "theta_FM_Final and thetaRef 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__": prescribedRot1DOFTestFunction( True, np.pi/6, # thetaInit 2*np.pi/3, # thetaRef 0.008, # thetaDDotMax 1e-12 # accuracy )