Source code for test_WaypointReference

#
#  ISC License
#
#  Copyright (c) 2016, 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:        waypointReference
#   Author:             Riccardo Calaon
#   Creation Date:      March 14, 2021
#


import os

import numpy as np
import pytest
from Basilisk.architecture import bskLogging
from Basilisk.fswAlgorithms import waypointReference
from Basilisk.utilities import RigidBodyKinematics as rbk
# Import all of the modules that we are going to be called in this simulation
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import macros
from Basilisk.utilities import unitTestSupport

path = os.path.dirname(os.path.abspath(__file__))
dataFileName = None
# The following 'parametrize' function decorator provides the parameters and expected results for each
# of the multiple test runs for this test.
[docs] @pytest.mark.parametrize("attType", [0, 1, 2]) @pytest.mark.parametrize("MRPswitching", [True, False]) @pytest.mark.parametrize("useReferenceFrame", [True, False]) @pytest.mark.parametrize("accuracy", [1e-12]) # provide a unique test method name, starting with test_ def test_waypointReference(show_plots, attType, MRPswitching, useReferenceFrame, accuracy): r""" **Validation Test Description** This unit test script tests the capability of the WaypointReference module to correctly read time-tagged attitude parameters, angular rates and angular accelerations from a text file. First a text file is generated that contains a sequence of time-tagged attitude parameters, angular rates and angular accelerations; subsequently, the same file is fed to the waypointReference module. The module is tested against all the attitude types that it supports: - MRPs - Euler Parameters (quaternion) [q0, q1, q2, q3] - Euler Parameters (quaternion) [q1, q2, q3, qs] and with angular rates and accelerations that can be expressed either in the inertial frame N or in the reference frame R. This unit test writes 5 time-tagged attitudes at times t = [1.0, 2.0, 3.0, 4.0, 5.0]s. Real values of attitude parameters, angular rates and angular accelerations in inertial frames are stored in ``attReal_RN``, ``omegaReal_RN_N`` and ``omegaDotReal_RN_N`` respectively. **Test Parameters** Args: attType (int): 0 - MRPs; 1 - EP [q0, q1, q2, q3]; 2 - [q1, q2, q3, qs] MRPswitching : False: every waypoint is within 180 deg from the inertial frame; True: some waipoints exceed 180 deg rotation from the inertial frame and ``attRefOutMsg.sigma_RN`` presents a discontinuity in correspondence of MRP switching; useReferenceFrame (bool): False: ang. rates and accelerations expressed in inertial frame; True: ang. rates and accelerations expressed in reference frame; accuracy (float): absolute accuracy value used in the validation tests **Description of Variables Being Tested** This unit test checks the correctness of the output attitude reference message - ``attRefMsg`` compared to the real values stored in the data file ``attReal_RN``, ``omegaReal_RN_N`` and ``omegaDotReal_RN_N``. The simulation is run with a sampling frequency of 0.25 s, which is higher than the frequency with which the attitude waypoints are saved in the data file (1.0 s), starting at t = 0. For t < 1.0 s we check that the attitude in ``attRefMsg`` coincides with ``attReal_RN`` at time t = 1.0 s, while rates and accelerations in ``attRefMsg`` are zero. For t > 5.0 s we check that the attitude in ``attRefMsg`` coincides with ``attReal_RN`` at time t = 5.0 s, while rates and accelerations in ``attRefMsg`` are zero. For 1.0 s <= t <= 5.0 s we check that the attitude, rates and accelerations in ``attRefMsg`` coincide with the linear interpolation of ``attReal_RN``, ``omegaReal_RN_N`` and ``omegaDotReal_RN_N``. """ # each test method requires a single assert method to be called [testResults, testMessage] = waypointReferenceTestFunction(attType, MRPswitching, useReferenceFrame, accuracy) global dataFileName if os.path.exists(dataFileName): os.remove(dataFileName) assert testResults < 1, testMessage
def waypointReferenceTestFunction(attType, MRPswitching, useReferenceFrame, accuracy): bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages unitTaskName = "unitTask" # arbitrary name (don't change) unitProcessName = "TestProcess" # arbitrary name (don't change) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread dtSeconds = 0.25 simTimeSeconds = 6 testProcessRate = macros.sec2nano(dtSeconds) simulationTime = macros.sec2nano(simTimeSeconds) testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # create the simulation data file # dataFileName global dataFileName dataFileName = "data" + str(attType) + str(int(MRPswitching)) if useReferenceFrame == True: dataFileName += "R.txt" else: dataFileName += "N.txt" dataFileName = os.path.join(path, dataFileName) delimiter = "," fDataFile = open(dataFileName, "w+") # create the datafile and store the real attitude, rate and acceleration values t = [] attReal_RN = [] omegaReal_RN_N = [] omegaDotReal_RN_N = [] for i in range(0, 5): t.append(i + 1) if MRPswitching: s = 0.8 else: s = 0.1 attRealMRP = np.array([s + 0.05*i, 0.2 + 0.05*i, 0.3 + 0.05*i]) if np.linalg.norm(attRealMRP) <= 1: attReal_RN.append(attRealMRP) else: attReal_RN.append(-attRealMRP / (np.linalg.norm(attRealMRP))**2 ) omegaReal_RN_N.append(np.array([0.4 + 0.05*i, 0.5 + 0.05*i, 0.6 + 0.05*i])) omegaDotReal_RN_N.append(np.array([0.7 + 0.05*i, 0.8 + 0.05*i, 0.9 + 0.05*i])) lineString = str(t[-1]) + delimiter if attType == 0: lineString += str(attReal_RN[-1].tolist())[1:-1] + delimiter elif attType == 1: q = rbk.MRP2EP(attReal_RN[-1]) lineString += str(q.tolist())[1:-1] + delimiter elif attType == 2: q = rbk.MRP2EP(attReal_RN[-1]) qs = [q[1], q[2], q[3], q[0]] lineString += str([float(elem) for elem in qs])[1:-1] + delimiter else: print("Invalid attitude type") return if not useReferenceFrame: lineString += str(omegaReal_RN_N[-1].tolist())[1:-1] + delimiter + str(omegaDotReal_RN_N[-1].tolist())[1:-1] + '\n' else: RN = rbk.MRP2C(attReal_RN[-1]) omegaReal_RN_R = np.matmul(RN, omegaReal_RN_N[-1]) omegaDotReal_RN_R = np.matmul(RN, omegaDotReal_RN_N[-1]) lineString += str(omegaReal_RN_R.tolist())[1:-1] + delimiter + str(omegaDotReal_RN_R.tolist())[1:-1] + '\n' # write line on file fDataFile.write(lineString) # close file fDataFile.close() # Construct algorithm and associated C++ container testModule = waypointReference.WaypointReference() testModule.ModelTag = "testModule" # load the data path from the same folder where this python script is testModule.dataFileName = dataFileName testModule.delimiter = delimiter testModule.attitudeType = attType testModule.useReferenceFrame = useReferenceFrame testModule.headerLines = 0 # Add module to the task unitTestSim.AddModelToTask(unitTaskName, testModule) # Setup logging on the test module output message so that we get all the writes to it dataLog = testModule.attRefOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) # Need to call the self-init and cross-init methods unitTestSim.InitializeSimulation() unitTestSim.ConfigureStopTime(simulationTime) # Begin the simulation time run set above unitTestSim.ExecuteSimulation() # This pulls the sampling times from the simulation run. timeData = dataLog.times() * macros.NANO2SEC # Check if logged data matches the real attitude, rates and accelerations j = 0 sigma_RN = [[], [], []] # checking attitude msg for t < t_min for i in range(len(timeData)-1): for n in range(3): sigma_RN[n].append(dataLog.sigma_RN[i][n]) if timeData[i] < t[0]: if not unitTestSupport.isVectorEqual(dataLog.sigma_RN[i], attReal_RN[0], accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed attitude check at time t = {}".format(timeData[i])) if not unitTestSupport.isVectorEqual(dataLog.omega_RN_N[i], np.array([0.0, 0.0, 0.0]), accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed angular rate check at time t = {}".format(timeData[i])) if not unitTestSupport.isVectorEqual(dataLog.domega_RN_N[i], np.array([0.0, 0.0, 0.0]), accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed angular acceleration check at time t = {}".format(timeData[i])) # checking attitude msg for t_min <= t <= t_max elif timeData[i] >= t[0] and timeData[i] <= t[-1]: while (timeData[i] >= t[j] and timeData[i] <= t[j+1]) == False: j += 1 sigma_RN_int = np.array([0.0, 0.0, 0.0]) omega_RN_N_int = np.array([0.0, 0.0, 0.0]) omegaDot_RN_N_int = np.array([0.0, 0.0, 0.0]) # interpolating between attitudes for times t = timeData[i] if np.linalg.norm( attReal_RN[j+1] - attReal_RN[j] ) <= 1: sigma_RN_int = attReal_RN[j] + (attReal_RN[j+1] - attReal_RN[j]) / (t[j+1] - t[j]) * (timeData[i] - t[j]) else: attReal_RN_SS = -attReal_RN[j+1] / (np.linalg.norm(attReal_RN[j+1]))**2 sigma_RN_int = attReal_RN[j] + (attReal_RN_SS - attReal_RN[j]) / (t[j+1] - t[j]) * (timeData[i] - t[j]) omega_RN_N_int = omegaReal_RN_N[j] + (omegaReal_RN_N[j+1] - omegaReal_RN_N[j]) / (t[j+1] - t[j]) * (timeData[i] - t[j]) omegaDot_RN_N_int = omegaDotReal_RN_N[j] + (omegaDotReal_RN_N[j+1] - omegaDotReal_RN_N[j]) / (t[j+1] - t[j]) * (timeData[i] - t[j]) if not unitTestSupport.isVectorEqual(dataLog.sigma_RN[i], sigma_RN_int, accuracy): print(timeData[i], dataLog.sigma_RN[i], sigma_RN_int) testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed attitude check at time t = {}".format(timeData[i])) if not unitTestSupport.isVectorEqual(dataLog.omega_RN_N[i], omega_RN_N_int, accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed angular rate check at time t = {}".format(timeData[i])) if not unitTestSupport.isVectorEqual(dataLog.domega_RN_N[i], omegaDot_RN_N_int, accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed angular acceleration check at time t = {}".format(timeData[i])) # checking attitude msg for t < t_max else: if not unitTestSupport.isVectorEqual(dataLog.sigma_RN[i], attReal_RN[-1], accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed attitude check at time t = {}".format(timeData[i])) if not unitTestSupport.isVectorEqual(dataLog.omega_RN_N[i], [0.0, 0.0, 0.0], accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed angular rate check at time t = {}".format(timeData[i])) if not unitTestSupport.isVectorEqual(dataLog.domega_RN_N[i], [0.0, 0.0, 0.0], accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed angular acceleration check at time t = {}".format(timeData[i])) # print out success or failure message if testFailCount == 0: print("PASSED: " + testModule.ModelTag) else: print("FAILED: " + testModule.ModelTag) print(testMessages) return [testFailCount, ''.join(testMessages)] # # This statement below ensures that the unitTestScript can be run as a # stand-along python script # if __name__ == "__main__": waypointReferenceTestFunction( 2, # attType (0 -> MRP, 1 -> quaternion [q0, q1, q2, q3], 2 -> quaternion [q1, q2, q3, qs]) False, # MRPswitching False, # useReferenceFrame 1e-12) if os.path.exists(dataFileName): os.remove(dataFileName)