# 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 Support Script
#
import math
import os, errno
import numpy as np
import matplotlib as mpl
from datetime import datetime, timedelta
from Basilisk.architecture import messaging
from Basilisk.topLevelModules import pyswice
mpl.rc("figure", facecolor="white")
mpl.rc('xtick', labelsize=9)
mpl.rc('ytick', labelsize=9)
mpl.rc("figure", figsize=(5.75, 2.5))
mpl.rc('axes', labelsize=10)
mpl.rc('legend', fontsize=9)
mpl.rc('figure', autolayout=True)
mpl.rc('figure', max_open_warning=30)
mpl.rc('legend', loc='lower right')
import matplotlib.colors as colors
import matplotlib.cm as cmx
from Basilisk.utilities import macros
from Basilisk import __path__
bskPath = __path__[0]
try:
from Basilisk.utilities import tabulate as T
# '''
# del(T.LATEX_ESCAPE_RULES['$'])
# del(T.LATEX_ESCAPE_RULES['\\'])
# del(T.LATEX_ESCAPE_RULES['_'])
# del(T.LATEX_ESCAPE_RULES['{'])
# del(T.LATEX_ESCAPE_RULES['}'])
# '''
del (T.LATEX_ESCAPE_RULES[u'$'])
del (T.LATEX_ESCAPE_RULES[u'\\'])
del (T.LATEX_ESCAPE_RULES[u'_'])
del (T.LATEX_ESCAPE_RULES[u'{'])
del (T.LATEX_ESCAPE_RULES[u'}'])
from Basilisk.utilities.tabulate import *
except:
pass
[docs]def isVectorEqual(result, truth, accuracy):
"""function to check if a 3D vector is the same as the truth values"""
if foundNAN(result): return 0
if np.linalg.norm(result - truth) > accuracy:
return 0 # return 0 to indicate the array's are not equal
return 1 # return 1 to indicate the two array's are equal
[docs]def isArrayEqual(result, truth, dim, accuracy):
"""function to check if an array of values is the same as the truth values"""
# the result array is of dimension dim, no time stamp
# the truth array is of dimesion dim, no time stamp
if dim < 1:
print("Incorrect array dimension " + dim + " sent to isArrayEqual")
return 0
if len(result) == 0:
print("Result array was empty")
return 0
if len(truth) == 0:
print("Truth array was empty")
return 0
if foundNAN(result): return 0
for i in range(0, dim):
if math.fabs(result[i] - truth[i]) > accuracy:
return 0 # return 0 to indicate the array's are not equal
return 1 # return 1 to indicate the two array's are equal
[docs]def isArrayEqualRelative(result, truth, dim, accuracy):
"""Compare relative accuracy of two arracy"""
# the result array is of dimension dim, no time stamp
# the truth array is of dimesion dim, no time stamp
if dim < 1:
print("Incorrect array dimension " + dim + " sent to isArrayEqual")
return 0
if len(result) == 0:
print("Result array was empty")
return 0
if len(truth) == 0:
print("Truth array was empty")
return 0
if foundNAN(result): return 0
for i in range(0, dim):
if truth[i] == 0:
if result[i] == 0:
continue
else:
print("Truth array contains zero")
return 0
if math.fabs((result[i] - truth[i]) / truth[i]) > accuracy:
return 0 # return 0 to indicate the array's are not equal
return 1 # return 1 to indicate the two array's are equal
[docs]def isArrayZero(result, dim, accuracy):
"""function to check if an array of values are zero"""
# the result array is of dimension dim
if dim < 1:
print("Incorrect array dimension " + dim + " sent to isArrayEqual")
return 0
if len(result) == 0:
print("Result array was empty")
return 0
if foundNAN(result): return 0
for i in range(0, dim):
if (math.fabs(result[i]) > accuracy):
return 0 # return 0 to indicate the array's are not equal
return 1 # return 1 to indicate the two array's are equal
[docs]def compareVector(trueStates, dataStates, accuracy, msg, testFailCount, testMessages, ExpectedResult=1):
"""Compare two vector size and values and check absolute accuracy"""
if (len(trueStates) != len(dataStates)):
testFailCount += 1
testMessages.append("FAILED: " + msg + r" unequal data array sizes\n")
else:
if isVectorEqual(dataStates, trueStates, accuracy) != ExpectedResult:
testFailCount += 1
testMessages.append("FAILED: " + msg + r"\n")
return testFailCount, testMessages
[docs]def compareArray(trueStates, dataStates, accuracy, msg, testFailCount, testMessages):
"""Compare two arrays size and values and check absolute accuracy"""
if (len(trueStates) != len(dataStates)):
testFailCount += 1
testMessages.append("FAILED: " + msg + r" unequal data array sizes\n")
elif (len(trueStates) == 0 or len(dataStates) == 0):
testFailCount += 1
testMessages.append("FAILED: " + msg + r" data had empty arrays\n")
else:
for i in range(0, len(trueStates)):
# check a vector values
if not isArrayEqual(dataStates[i], trueStates[i], 3, accuracy):
testFailCount += 1
testMessages.append("FAILED: " + msg + "\n")
return testFailCount, testMessages
[docs]def compareArrayND(trueStates, dataStates, accuracy, msg, size, testFailCount, testMessages):
"""Compare two arrays of size N for size and values and check absolute accuracy"""
if len(trueStates) != len(dataStates):
testFailCount += 1
testMessages.append("FAILED: " + msg + r" unequal data array sizes\n")
elif len(trueStates) == 0 or len(dataStates) == 0:
testFailCount += 1
testMessages.append("FAILED: " + msg + r" data had empty arrays\n")
else:
for i in range(0, len(trueStates)):
# check a vector values
try:
data = dataStates[i].flatten()
except:
data = dataStates[i]
try:
trueValue = trueStates[i].flatten()
except:
trueValue = trueStates[i]
if not isArrayEqual(data, trueValue, size, accuracy):
testFailCount += 1
testMessages.append("FAILED: " + msg)
return testFailCount, testMessages
[docs]def compareArrayRelative(trueStates, dataStates, accuracy, msg, testFailCount, testMessages):
"""
Checks whether the relative distance between elements of a pullMessageLogData-derived array and a
truth array is below a provided accuracy, and return an error if not.
Args:
trueStates: iterable of size (m,n);
dataStates: iterable of size (m,n)
accuracy: Relative accuracy boundary
msg:
testFailCount:
testMessages:
"""
if (len(trueStates) != len(dataStates)):
testFailCount += 1
testMessages.append("FAILED: " + msg + r" unequal data array sizes\n")
elif len(trueStates) == 0 or len(dataStates) == 0:
testFailCount += 1
testMessages.append("FAILED: " + msg + r" data had empty arrays\n")
else:
for i in range(0, len(trueStates)):
# check a vector values
if not isArrayEqualRelative(dataStates[i], trueStates[i], 3, accuracy):
testFailCount += 1
testMessages.append("FAILED: " + msg + " at t=" + str(dataStates[i, 0] * macros.NANO2SEC) + r"sec\n")
return testFailCount, testMessages
[docs]def isDoubleEqual(result, truth, accuracy):
"""function to check if a double equals a truth value"""
if foundNAN(result): return 0
if math.fabs(result - truth) > accuracy:
return 0 # return 0 to indicate the doubles are not equal
return 1 # return 1 to indicate the doubles are equal
[docs]def isDoubleEqualRelative(result, truth, accuracy):
"""function to check if a double equals a truth value with relative tolerance"""
if foundNAN(result): return 0
if foundNAN(truth): return 0
if foundNAN(accuracy): return 0
if truth == 0:
print("truth is zero, cannot compare")
return 0
if math.fabs((truth - result) / truth) > accuracy:
return 0 # return 0 to indicate the doubles are not equal
return 1 # return 1 to indicate the doubles are equal
[docs]def compareDoubleArrayRelative(trueStates, dataStates, accuracy, msg, testFailCount, testMessages):
"""Compare two arrays of doubles for size and values and check relative accuracy"""
if len(trueStates) != len(dataStates):
testFailCount += 1
testMessages.append("FAILED: " + msg + r" unequal data array sizes\n")
elif len(trueStates) == 0 or len(dataStates) == 0:
testFailCount += 1
testMessages.append("FAILED: " + msg + r" data had empty arrays\n")
else:
for i in range(0, len(trueStates)):
# check a vector values
if not isDoubleEqualRelative(dataStates[i], trueStates[i], accuracy):
testFailCount += 1
testMessages.append("FAILED: " + msg + "\n")
return testFailCount, testMessages
[docs]def compareDoubleArray(trueStates, dataStates, accuracy, msg, testFailCount, testMessages):
"""Compare two arrays of doubles for size and values and check absolute accuracy"""
if len(trueStates) != len(dataStates):
testFailCount += 1
testMessages.append("FAILED: " + msg + r" unequal data array sizes\n")
elif len(trueStates) == 0 or len(dataStates) == 0:
testFailCount += 1
testMessages.append("FAILED: " + msg + r" data had empty arrays\n")
else:
for i in range(0, len(trueStates)):
# check a vector values
if not isDoubleEqual(dataStates[i], trueStates[i], accuracy):
testFailCount += 1
testMessages.append("FAILED: " + msg + "\n")
return testFailCount, testMessages
[docs]def writeTableLaTeX(tableName, tableHeaders, caption, array, path):
"""Take a list and return equivalent LaTeX table code"""
texFileName = path + "/../_Documentation/AutoTeX/" + tableName + ".tex"
if not os.path.exists(os.path.dirname(texFileName)):
try:
os.makedirs(os.path.dirname(texFileName))
except OSError as exc: # Guard against race condition
if exc.errno != errno.EEXIST:
raise
with open(texFileName, "w") as texTable:
table = tabulate(array,
tableHeaders,
tablefmt="latex",
numalign="center"
)
texTable.write(r'\begin{table}[htbp]')
texTable.write(r'\caption{' + caption + '}')
texTable.write(r'\label{tbl:' + tableName + '}')
texTable.write(r'\centering')
texTable.write(table)
texTable.write(r'\end{table}')
texTable.close()
return
[docs]def writeTeXSnippet(snippetName, texSnippet, path):
"""Write a LaTeX snippet to a file"""
texFileName = path + "/../_Documentation/AutoTeX/" + snippetName + ".tex"
if not os.path.exists(os.path.dirname(texFileName)):
try:
os.makedirs(os.path.dirname(texFileName))
except OSError as exc: # Guard against race condition
if exc.errno != errno.EEXIST:
raise
with open(texFileName, "w") as fileHandler:
fileHandler.write(texSnippet)
fileHandler.close()
return
[docs]def foundNAN(array):
"""check if an array contains NAN values"""
if (np.isnan(np.sum(array))):
print("Warning: found NaN value.")
return 1 # return 1 to indicate a NaN value was found
return 0
[docs]def getLineColor(idx, maxNum):
"""pick a nicer color pattern to plot 3 vector components"""
values = list(range(0, maxNum + 2))
colorMap = mpl.pyplot.get_cmap('gist_earth')
cNorm = colors.Normalize(vmin=0, vmax=values[-1])
scalarMap = cmx.ScalarMappable(norm=cNorm, cmap=colorMap)
return scalarMap.to_rgba(values[idx + 1])
[docs]def np2EigenMatrix3d(mat):
"""convert 3D numpy matrix to Eigen matrix"""
return [
[mat[0], mat[1], mat[2]]
, [mat[3], mat[4], mat[5]]
, [mat[6], mat[7], mat[8]]
]
[docs]def np2EigenVectorXd(vec):
"""Convert numpy to Eigen vector"""
npVec = []
for item in vec:
npVec.extend([[item]])
return npVec
[docs]def npList2EigenXdVector(list):
"""Conver a list of arrays to a list of eigen values"""
eigenList = messaging.Eigen3dVector(len(list))
c = 0
for pos in list:
eigenList[c] = np2EigenVectorXd(pos)
c += 1
return eigenList
[docs]def EigenVector3d2np(eig):
"""convert Eigen vector3d to numpy"""
return np.array([eig[0][0], eig[1][0], eig[2][0]])
[docs]def pullVectorSetFromData(inpMat):
"""extract the vector data set from a data matrix where the 1st column is the time information"""
outMat = np.array(inpMat).transpose()
return outMat[1:].transpose()
[docs]def addTimeColumn(time, data):
"""Add a time column to the data set"""
return np.transpose(np.vstack([[time], np.transpose(data)]))
[docs]def decimalYearToDateTime(start):
"""convert a decimal Year format to a regular dataTime object"""
year = int(start)
rem = start - year
base = datetime(year, 1, 1)
return base + timedelta(seconds=(base.replace(year=base.year + 1) - base).total_seconds() * rem)
[docs]def timeStringToGregorianUTCMsg(DateSpice, **kwargs):
"""convert a general time/date string to a gregoarian UTC msg object"""
# set the data path
if 'dataPath' in kwargs:
dataPath = kwargs['dataPath']
if not isinstance(dataPath, str):
print('ERROR: dataPath must be a string argument')
exit(1)
else:
dataPath = bskPath + '/supportData/EphemerisData/' # default value
# load spice kernel and convert the string into a UTC date/time string
pyswice.furnsh_c(dataPath + 'naif0012.tls')
et = pyswice.new_doubleArray(1)
pyswice.str2et_c(DateSpice, et)
etEpoch = pyswice.doubleArray_getitem(et, 0)
ep1 = pyswice.et2utc_c(etEpoch, 'C', 6, 255, "Yo")
pyswice.unload_c(dataPath + 'naif0012.tls') # leap second file
# convert UTC string to datetime object
datetime_object = datetime.strptime(ep1, '%Y %b %d %H:%M:%S.%f')
# populate the epochMsg with the gregorian UTC date/time information
epochMsgStructure = messaging.EpochMsgPayload()
epochMsgStructure.year = datetime_object.year
epochMsgStructure.month = datetime_object.month
epochMsgStructure.day = datetime_object.day
epochMsgStructure.hours = datetime_object.hour
epochMsgStructure.minutes = datetime_object.minute
epochMsgStructure.seconds = datetime_object.second + datetime_object.microsecond / 1e6
epochMsg = messaging.EpochMsg().write(epochMsgStructure)
epochMsg.this.disown()
return epochMsg
[docs]def checkMethodKeyword(karglist, kwargs):
"""loop through list of method keyword arguments and make sure that an approved keyword is used."""
for key in kwargs:
if key not in karglist:
print('ERROR: you tried to use an incorrect keyword ' + key + '. Options include:')
print(karglist)
exit(1)
[docs]def removeTimeFromData(dataList):
"""pull out the time column out of a 4xN data list"""
return (dataList.transpose()[1:len(dataList[0])]).transpose()
[docs]def samplingTime(simTime, baseTimeStep, numDataPoints):
"""
Given a simulation duration, this routine returns
a sampling time that yields the closest integer match to a desired number of sampling points
Args:
simTime: [ns] total simulation duration
baseTimeStep: [ns] baseline sampling period
numDataPoints: nominal desired number of data points over the simulation duration
"""
deltaTime = math.floor(simTime / baseTimeStep / (numDataPoints - 1)) * baseTimeStep
if deltaTime < 1:
deltaTime = 1
return deltaTime