Source code for test_BSpline


# 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.


#
# BSpline Unit Test
#
# Purpose:  Tests the BSpline interpolating function
# Author:   Riccardo Calaon
# Creation Date:  Oct 10 2021
#

import inspect
import os

import numpy as np
import pytest
from Basilisk.architecture import BSpline

filename = inspect.getframeinfo(inspect.currentframe()).filename
path = os.path.dirname(os.path.abspath(filename))
# 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("P", [5, 6]) @pytest.mark.parametrize("XDot_flag", [False, True]) @pytest.mark.parametrize("XDDot_flag", [False, True]) @pytest.mark.parametrize("accuracy", [1e-6]) def test_BSpline(show_plots, P, XDot_flag, XDDot_flag, accuracy): r""" **Validation Test Description** This unit test script tests the capability of the BSpline function to correctly interpolate a series of points in 3 dimensions. The coordinates of these 7 points are stored in 3 numpy arrays: X1 = np.array([0, 1, 2, 3, 4, 5, 6]) X2 = np.array([5, 4, 3, 2, 1, 0, 1]) X3 = np.array([3, 2, 1, 2, 3, 4, 5]). The input arrays are initialized through ``Input = BSpline.InputDataSet(X1, X2, X3)``. The time tags at which each waypoint is to be hit are provided through ``Input.setT([0, 2, 3, 5, 7, 8, 10])``. Alternatively, it is possible to specify the average velocity norm through ``Input.setAvgXDot()``. The endpoint derivatives are specified through the methods: - ``Input.setXDot_0()`` for starting point first-order derivative; - ``Input.setXDot_N()`` for last point first-order derivative; - ``Input.setXDDot_0()`` for starting point second-order derivative; - ``Input.setXDDot_N()`` for last point second-order derivative. Each method to specify the derivatives takes in a 3-dimensional numpy array. The output data structure is created with ``Output = BSpline.OutputDataSet()``. The interpolation happens calling the method ``BSpline.interpolate(Input, N, P, Output)`` where: - N is the desired number of equally spaced data points in the interpolated function; - P is the polynomial order of the B-Spline function. The order should be at least 3 when first-order derivatives are specified, and 5 when second-order derivatives are specified. The maximum oder is P = n + k - 1, with n being the number of waypoints and k being the number of endpoint derivatives that are being specified. **Test Parameters** As this is a parameterized unit test, note that the test case parameters values are shown automatically in the pytest HTML report. This sample script has the parameters param1 and param 2. Provide a description of what each parameter controls. This is a convenient location to include the accuracy variable used in the validation test. Args: P (int): polynomial order of the B-Spline curve; XDot_flag (bool) : whether the first-order end point derivatives should be specified; XDDot_flag (bool) : whether the second-order end point derivatives should be specified; accuracy (float): absolute accuracy value used in the validation tests. **Description of Variables Being Tested** This unit test checks the correctness of the interpolated function: - a check is performed on whether or not each waypoint is hit at the specified time; - when the derivatives are specified, it checks whether the starting point derivative actually matches the input derivative. """ # each test method requires a single assert method to be called [testResults, testMessage] = BSplineTestFunction(P, XDot_flag, XDDot_flag, accuracy) assert testResults < 1, testMessage
def BSplineTestFunction(P, XDot_flag, XDDot_flag, accuracy): testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages X1 = np.array([0, 1, 2, 3, 4, 5, 6]) X2 = np.array([5, 4, 3, 2, 1, 0, 1]) X3 = np.array([3, 2, 1, 2, 3, 4, 5]) Input = BSpline.InputDataSet(X1, X2, X3) Input.setT([0, 2, 3, 5, 7, 8, 10]) if XDot_flag: Input.setXDot_0([0, 0, 0]) Input.setXDot_N([0, 0, 0]) if XDDot_flag: Input.setXDDot_0([0, 0, 0]) Input.setXDDot_N([0.2, 0, 0]) Output = BSpline.OutputDataSet() BSpline.interpolate(Input, 101, P, Output) for i in range(len(Output.T)): for j in range(len(Input.T)): if abs(Output.T[i][0] - Input.T[j][0]) < accuracy: if not abs(Output.X1[i][0] - X1[j]) < accuracy: testFailCount += 1 testMessages.append("FAILED: BSpline." + " Function of order {} failed coordinate #1 check at time t = {}".format(P,Input.T[j][0])) if not abs(Output.X2[i][0] - X2[j]) < accuracy: testFailCount += 1 testMessages.append("FAILED: BSpline." + " Function of order {} failed coordinate #2 check at time t = {}".format(P,Input.T[j][0])) if not abs(Output.X3[i][0] - X3[j]) < accuracy: testFailCount += 1 testMessages.append("FAILED: BSpline." + " Function of order {} failed coordinate #3 check at time t = {}".format(P,Input.T[j][0])) if XDot_flag: if not ((abs(Output.XD1[0][0]-Input.XDot_0[0][0]) < accuracy) and (abs(Output.XD2[0][0]-Input.XDot_0[1][0]) < accuracy) and (abs(Output.XD3[0][0]-Input.XDot_0[2][0]) < accuracy)): testFailCount += 1 testMessages.append("FAILED: BSpline." + " Function of order {} failed first derivative at starting point".format(P)) if XDDot_flag: if not ((abs(Output.XDD1[0][0]-Input.XDDot_0[0][0]) < accuracy) and (abs(Output.XDD2[0][0]-Input.XDDot_0[1][0]) < accuracy) and (abs(Output.XDD3[0][0]-Input.XDDot_0[2][0]) < accuracy)): testFailCount += 1 testMessages.append("FAILED: BSpline." + " Function of order {} failed second derivative at starting point".format(P)) return [testFailCount, ''.join(testMessages)] # # This statement below ensures that the unitTestScript can be run as a # stand-along python script # if __name__ == "__main__": BSplineTestFunction( 5, # polynomial order True, # XDot_flag False, # XDDot_flag 1e-6)