# 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.
import math
import numpy as np
import pytest
from Basilisk import __path__
from Basilisk.utilities import deprecated, macros
from Basilisk.utilities import simHelpers as _simHelpers
bskPath = __path__[0]
_SIM_HELPER_REMOVAL_DATE = "2027/06/23"
def _deprecatedSimHelper(func):
"""Apply standard Basilisk deprecation behavior to a moved helper."""
return deprecated.deprecated(
_SIM_HELPER_REMOVAL_DATE, f"Use simHelpers.{func.__name__} instead."
)(func)
@_deprecatedSimHelper
def EigenVector3d2list(*args, **kwargs):
return _simHelpers.EigenVector3d2list(*args, **kwargs)
@_deprecatedSimHelper
def EigenVector3d2np(*args, **kwargs):
return _simHelpers.EigenVector3d2np(*args, **kwargs)
@_deprecatedSimHelper
def addTimeColumn(*args, **kwargs):
return _simHelpers.addTimeColumn(*args, **kwargs)
@_deprecatedSimHelper
def checkMethodKeyword(*args, **kwargs):
return _simHelpers.checkMethodKeyword(*args, **kwargs)
@_deprecatedSimHelper
def columnToRowList(*args, **kwargs):
return _simHelpers.columnToRowList(*args, **kwargs)
@_deprecatedSimHelper
def decimalYearToDateTime(*args, **kwargs):
return _simHelpers.decimalYearToDateTime(*args, **kwargs)
@_deprecatedSimHelper
def flattenList(*args, **kwargs):
return _simHelpers.flattenList(*args, **kwargs)
@_deprecatedSimHelper
def getLineColor(*args, **kwargs):
return _simHelpers.getLineColor(*args, **kwargs)
@_deprecatedSimHelper
def getScenarioFigureFileName(*args, **kwargs):
return _simHelpers.getScenarioFigureFileName(*args, **kwargs)
@_deprecatedSimHelper
def np2EigenMatrix3d(*args, **kwargs):
return _simHelpers.np2EigenMatrix3d(*args, **kwargs)
@_deprecatedSimHelper
def np2EigenVectorXd(*args, **kwargs):
return _simHelpers.np2EigenVectorXd(*args, **kwargs)
@_deprecatedSimHelper
def npList2EigenXdVector(*args, **kwargs):
return _simHelpers.npList2EigenXdVector(*args, **kwargs)
@_deprecatedSimHelper
def pullVectorSetFromData(*args, **kwargs):
return _simHelpers.pullVectorSetFromData(*args, **kwargs)
@_deprecatedSimHelper
def removeTimeFromData(*args, **kwargs):
return _simHelpers.removeTimeFromData(*args, **kwargs)
@_deprecatedSimHelper
def samplingTime(*args, **kwargs):
return _simHelpers.samplingTime(*args, **kwargs)
@_deprecatedSimHelper
def saveFigurePDF(*args, **kwargs):
return _simHelpers.saveFigurePDF(*args, **kwargs)
@_deprecatedSimHelper
def saveScenarioFigure(*args, **kwargs):
return _simHelpers.saveScenarioFigure(*args, **kwargs)
@_deprecatedSimHelper
def saveScenarioGraphvizFigure(*args, **kwargs):
return _simHelpers.saveScenarioGraphvizFigure(*args, **kwargs)
@_deprecatedSimHelper
def timeStringToGregorianUTCMsg(*args, **kwargs):
return _simHelpers.timeStringToGregorianUTCMsg(*args, **kwargs)
@_deprecatedSimHelper
def writeFigureLaTeX(*args, **kwargs):
return _simHelpers.writeFigureLaTeX(*args, **kwargs)
@_deprecatedSimHelper
def writeTableLaTeX(*args, **kwargs):
return _simHelpers.writeTableLaTeX(*args, **kwargs)
@_deprecatedSimHelper
def writeTeXSnippet(*args, **kwargs):
return _simHelpers.writeTeXSnippet(*args, **kwargs)
[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 arrays"""
# 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 compareListRelative(
trueStates, dataStates, accuracy, msg, testFailCount, testMessages
):
"""Compare two row lists of 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:
if not trueStates == pytest.approx(dataStates, rel=accuracy):
testFailCount += 1
testMessages.append("FAILED: " + msg + "\n")
return testFailCount, testMessages
[docs]
def compareList(trueStates, dataStates, accuracy, msg, testFailCount, testMessages):
"""Compare two row lists of 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:
if not trueStates == pytest.approx(dataStates, abs=accuracy):
testFailCount += 1
testMessages.append("FAILED: " + msg + "\n")
return testFailCount, testMessages
[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