Source code for test_ConstrainedAttitudeManeuver

#
#  ISC License
#
#  Copyright (c) 2022, 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:        constrainedAttitudeManeuver
#   Author:             Riccardo Calaon
#   Creation Date:      April 16, 2022
#


import os

import numpy as np
import pytest
from Basilisk.architecture import BSpline
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import constrainedAttitudeManeuver
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


def shadowSetMap(sigma, switch):
    sigma = np.array(sigma)
    if switch:
        s2 = (sigma[0]**2 + sigma[1]**2 + sigma[2]**2)
        if not s2 == 0:
            return -sigma / s2
        else:
            return sigma
    else:
        return sigma


class constraint:
    def __init__(self, axis, color):
        self.axis =  axis / np.linalg.norm(axis)
        self.color = color
        

class node:
    def __init__(self, sigma_BN, constraints, **kwargs):
        self.sigma_BN = np.array(sigma_BN)
        s = np.linalg.norm(self.sigma_BN)
        if s > 1:
            self.sigma_BN = shadowSetMap(sigma_BN, True)  # mapping to shadow set if |sigma| > 1
        if np.abs(s-1) < 1e-5:
            s = 1.
        self.isBoundary = False
        self.s = s
        if s == 1:
            self.isBoundary = True
        self.isFree = True
        self.color = ''
        self.neighbors = {}
        self.heuristic = 0
        self.priority = 0
        self.backpointer = self
        # check cosntraint compliance
        sigma_tilde = np.array([ [0,           -sigma_BN[2], sigma_BN[1]],
                                 [sigma_BN[2],      0,      -sigma_BN[0]],
                                 [-sigma_BN[1], sigma_BN[0],     0      ] ])
        BN = np.identity(3) + ( 8*np.matmul(sigma_tilde, sigma_tilde) -4*(1-s**2)*sigma_tilde ) / (1 + s**2)**2
        NB = BN.transpose()
        # checking for keepOut constraint violation
        if 'keepOut_b' in kwargs:
            for i in range(len(kwargs['keepOut_b'])):
                b_B = np.array(kwargs['keepOut_b'][i])
                b_N = np.matmul(NB, b_B)
                for c in constraints['keepOut']:
                    if np.dot(b_N, c.axis) >= np.cos(kwargs['keepOut_fov'][i]):
                        self.isFree = False
                        self.color = c.color
                        return
        # checking for keepIn constraint violation (at least one SS must see the Sun)
        if 'keepIn_b' in kwargs:
            b_N = []
            for i in range(len(kwargs['keepIn_b'])):
                b_B = np.array(kwargs['keepIn_b'][i])
                b_N.append(np.matmul(NB, b_B))
            isIn = False
            for c in constraints['keepIn']:
                for i in range(len(b_N)):
                    if np.dot(b_N[i], c.axis) >= np.cos(kwargs['keepIn_fov'][i]):
                        isIn = True
                        self.color = c.color
            if not isIn:
                self.isFree = False


def distanceCart(n1, n2):
    d1 = np.linalg.norm(n1.sigma_BN - n2.sigma_BN)
    sigma1norm2 = n1.sigma_BN[0]**2 + n1.sigma_BN[1]**2 + n1.sigma_BN[2]**2
    sigma2norm2 = n2.sigma_BN[0]**2 + n2.sigma_BN[1]**2 + n2.sigma_BN[2]**2
    if sigma2norm2 > 1e-8:
        sigma2_SS = shadowSetMap(n2.sigma_BN, True)
        d2 = np.linalg.norm(n1.sigma_BN - sigma2_SS)
    else:
        d2 = d1
    if sigma1norm2 > 1e-8:
        sigma1_SS = shadowSetMap(n1.sigma_BN, True)
        d3 = np.linalg.norm(sigma1_SS - n2.sigma_BN)
    else:
        d3 = d1
    if sigma1norm2 > 1e-8 and sigma2norm2 > 1e-8:
        d4 = np.linalg.norm(sigma1_SS - sigma2_SS)
    else:
        d4 = d1

    return min(d1, d2, d3, d4)


def distanceMRP(n1, n2):    
    s1 = n1.sigma_BN
    s2 = n2.sigma_BN
    sigma1norm2 = n1.sigma_BN[0]**2 + n1.sigma_BN[1]**2 + n1.sigma_BN[2]**2
    sigma2norm2 = n2.sigma_BN[0]**2 + n2.sigma_BN[1]**2 + n2.sigma_BN[2]**2

    D = 1 + (sigma1norm2*sigma2norm2)**2 + 2*np.dot(s1, s2)

    if abs(D) < 1e-5:
        s2 = shadowSetMap(s2, True)
        sigma2norm2 = 1 / sigma2norm2
        D = 1 + (sigma1norm2*sigma2norm2)**2 + 2*np.dot(s1, s2)

    s12 = ( (1-sigma2norm2)*s1 - (1-sigma1norm2)*s2 + 2*np.cross(s1, s2) ) / D
    sigma12norm2 = np.linalg.norm(s12)**2

    if sigma12norm2 > 1:
        s12 = shadowSetMap(s12, True)

    return 4*np.arctan(np.linalg.norm(s12))


def mirrorFunction(i, j, k):
    return [ [i, j, k], [-i, j, k], [i, -j, k], [i, j, -k], [-i, -j, k], [-i, j, -k], [i, -j, -k], [-i, -j, -k] ]


def neighboringNodes(i, j, k):
    return [ [i-1,  j,  k], [i+1,  j,  k], [i,  j-1,  k], [i,  j+1,  k], [i,  j,  k-1], [i,  j,  k+1],
             [i-1, j-1, k], [i+1, j-1, k], [i-1, j+1, k], [i+1, j+1, k], [i-1, j, k-1], [i+1, j, k-1],
             [i-1, j, k+1], [i+1, j, k+1], [i, j-1, k-1], [i, j+1, k-1], [i, j-1, k+1], [i, j+1, k+1],
             [i-1,j-1,k-1], [i+1,j-1,k-1], [i-1,j+1,k-1], [i-1,j-1,k+1], [i+1,j+1,k-1], [i+1,j-1,k+1], [i-1,j+1,k+1], [i+1,j+1,k+1] ]


def generateGrid(n_start, n_goal, N, constraints, data):
    
    u = np.linspace(0, 1, N, endpoint = True)
    nodes = {}

    # add internal nodes (|sigma| <= 1)
    for i in range(N):
        for j in range(N):
            for k in range(N):
                if (u[i]**2+u[j]**2+u[k]**2) <= 1:
                    for m in mirrorFunction(i, j, k):
                        if (m[0], m[1], m[2]) not in nodes:
                            nodes[(m[0], m[1], m[2])] = node([np.sign(m[0])*u[i], np.sign(m[1])*u[j], np.sign(m[2])*u[k]], constraints, **data)
    # add missing boundary nodes (|sigma| = 1)
    for i in range(N-1):
        for j in range(N-1):
            for k in range(N-1):
                if (i, j, k) in nodes:
                    if not nodes[(i, j, k)].isBoundary:
                        if (i+1, j, k) not in nodes:
                            for m in mirrorFunction(i+1, j, k):
                                if (m[0], m[1], m[2]) not in nodes:
                                    nodes[(m[0], m[1], m[2])] = node([np.sign(m[0])*(1-u[j]**2-u[k]**2)**0.5, np.sign(m[1])*u[j], np.sign(m[2])*u[k]], constraints, **data)
                        if (i, j+1, k) not in nodes:
                            for m in mirrorFunction(i, j+1, k):
                                if (m[0], m[1], m[2]) not in nodes:
                                    nodes[(m[0], m[1], m[2])] = node([np.sign(m[0])*u[i], np.sign(m[1])*(1-u[i]**2-u[k]**2)**0.5, np.sign(m[2])*u[k]], constraints, **data)
                        if (i, j, k+1) not in nodes:
                            for m in mirrorFunction(i, j, k+1):
                                if (m[0], m[1], m[2]) not in nodes:
                                    nodes[(m[0], m[1], m[2])] = node([np.sign(m[0])*u[i], np.sign(m[1])*u[j], np.sign(m[2])*(1-u[i]**2-u[j]**2)**0.5], constraints, **data)
    
    # link nodes
    for key1 in nodes:
        i = key1[0]
        j = key1[1]
        k = key1[2]
        # link nodes to immediate neighbors
        for n in neighboringNodes(i, j, k):
            key2 = (n[0], n[1], n[2])
            if key2 in nodes:
                if nodes[key1].isFree and nodes[key2].isFree:
                    nodes[key1].neighbors[key2] = nodes[key2]
        # link boundary nodes to neighbors of respective shadow sets
        if nodes[key1].isBoundary:
            if (-i, -j, -k) in nodes:
                for key2 in nodes[(-i, -j, -k)].neighbors:
                    if nodes[key1].isFree and nodes[key2].isFree and key2 not in nodes[key1].neighbors:
                        nodes[key1].neighbors[key2] = nodes[key2]

    # add start and goal nodes
    # looking for closest nodes to start and goal
    ds = 10
    dg = 10
    for key in nodes:
        if nodes[key].isFree:
            d1 = distanceCart(nodes[key], n_start)
            if abs(d1-ds) < 1e-6:
                if np.linalg.norm(nodes[key].sigma_BN) < np.linalg.norm(nodes[key_s].sigma_BN):
                    ds = d1
                    n_s = nodes[key]
                    key_s = key
            else:
                if d1 < ds:
                    ds = d1
                    n_s = nodes[key]
                    key_s = key
            d2 = distanceCart(nodes[key], n_goal)
            if abs(d2-dg) < 1e-6:
                if np.linalg.norm(nodes[key].sigma_BN) < np.linalg.norm(nodes[key_g].sigma_BN):
                    dg = d2
                    n_g = nodes[key]
                    key_g = key
            else:
                if d2 < dg:
                    dg = d2
                    n_g = nodes[key]
                    key_g = key
    for key in n_s.neighbors:
        n_start.neighbors[key] = n_s.neighbors[key]
    for key in n_g.neighbors:
        nodes[key].neighbors[key_g] = n_goal
    nodes[key_s] = n_start 
    nodes[key_g] = n_goal

    return nodes


def backtrack(n, n_start):
    if n == n_start:
        path = [n]
        return path
    else:
        path = backtrack(n.backpointer, n_start)
        path.append(n)
        return path


def pathHandle(path, avgOmega):

    T = [0]
    S = 0
    for n in range(len(path)-1):
        T.append(T[n] + distanceCart(path[n], path[n+1]))
        S += T[n+1] - T[n]

    X1 = []
    X2 = []
    X3 = []

    shadowSet = False
    for n in range(len(path)-1):
        if not shadowSet:
            sigma = path[n].sigma_BN
        else:
            if unitTestSupport.isVectorEqual(path[n].sigma_BN, [0,0,0], 1e-6):
                for m in range(0,n):
                    s2 = (X1[m]**2 + X2[m]**2 + X3[m]**2)**0.5
                    X1[m] = -X1[m] / s2
                    X2[m] = -X2[m] / s2
                    X3[m] = -X3[m] / s2
                shadowSet = not shadowSet
            sigma = rbk.MRPswitch(path[n].sigma_BN, 0)
        delSigma = path[n+1].sigma_BN - path[n].sigma_BN
        if (np.linalg.norm(delSigma) > 1):
            shadowSet = not shadowSet
        X1.append(sigma[0])
        X2.append(sigma[1])
        X3.append(sigma[2])
    if shadowSet:
        sigma = rbk.MRPswitch(path[-1].sigma_BN, 0)
    else:
        sigma = path[-1].sigma_BN
    X1.append(sigma[0])
    X2.append(sigma[1])
    X3.append(sigma[2])

    Input = BSpline.InputDataSet(X1, X2, X3)
    Input.setT( np.array(T) * 4 * S / (T[-1] * avgOmega) )

    return Input


def spline(Input, omegaS, omegaG):

    sigmaS = [Input.X1[0][0],  Input.X2[0][0],  Input.X3[0][0]]
    sigmaG = [Input.X1[-1][0], Input.X2[-1][0], Input.X3[-1][0]]
    sigmaDotS = rbk.dMRP(sigmaS, omegaS)
    sigmaDotG = rbk.dMRP(sigmaG, omegaG)

    Input.setXDot_0(sigmaDotS)
    Input.setXDot_N(sigmaDotG)

    Output = BSpline.OutputDataSet()
    BSpline.interpolate(Input, 100, 4, Output)

    return Output


def computeTorque(sigma, sigmaDot, sigmaDDot, I):

    omega = rbk.dMRP2Omega(sigma, sigmaDot)
    omegaDot = rbk.ddMRP2dOmega(sigma, sigmaDot, sigmaDDot)

    return np.matmul(I, omegaDot) + np.cross(omega, np.matmul(I, omega))


def effortEvaluation(Output, I):

    effort = 0
    sigma     = [Output.X1[0][0], Output.X2[0][0], Output.X3[0][0]]
    sigmaDot  = [Output.XD1[0][0], Output.XD2[0][0], Output.XD3[0][0]]
    sigmaDDot = [Output.XDD1[0][0], Output.XDD2[0][0], Output.XDD3[0][0]]
    L_a = computeTorque(sigma, sigmaDot, sigmaDDot, I)

    for n in range(len(Output.T)-1):
        sigma     = [Output.X1[n+1][0], Output.X2[n+1][0], Output.X3[n+1][0]]
        sigmaDot  = [Output.XD1[n+1][0], Output.XD2[n+1][0], Output.XD3[n+1][0]]
        sigmaDDot = [Output.XDD1[n+1][0], Output.XDD2[n+1][0], Output.XDD3[n+1][0]]
        L_b = computeTorque(sigma, sigmaDot, sigmaDDot, I)
        effort += (np.linalg.norm(L_a) + np.linalg.norm(L_b)) * (Output.T[n+1][0] - Output.T[n][0]) / 2

        L_a = L_b

    return effort


def AStar(nodes, n_start, n_goal):

    for key in nodes:
        nodes[key].heuristic = distanceCart(nodes[key], n_goal)

    O = [n_start]
    C = []
    n = 0

    while O[0] != n_goal and n < 10000:
        n += 1
        C.append(O[0])
        for key in O[0].neighbors:
            if nodes[key] not in C:
                p = nodes[key].heuristic + distanceCart(O[0], nodes[key]) + O[0].priority - O[0].heuristic
                if nodes[key] in O:
                    if p < nodes[key].priority:
                        nodes[key].priority = p
                        nodes[key].backpointer = O[0]
                else:
                    nodes[key].priority = p
                    nodes[key].backpointer = O[0]
                    O.append(nodes[key])
        O.pop(0)

        if not O:
            print("Dead end")
        else:
            O.sort(key = lambda x: x.priority)

    path = backtrack(O[0], n_start)

    return path


def effortBasedAStar(nodes, n_start, n_goal, omegaS, omegaG, avgOmega, I):

    O = [n_start]
    C = []
    n = 0

    while O[0] != n_goal and n < 10000:
        n += 1
        print(n)
        C.append(O[0])
        for key in O[0].neighbors:
            if nodes[key] not in C:
                path = backtrack(O[0], n_start)
                path.append(nodes[key])
                if nodes[key] != n_goal:
                    path.append(n_goal)
                Input = pathHandle(path, avgOmega)
                Output = spline(Input, omegaS, omegaG)
                p = effortEvaluation(Output, I)
                if nodes[key] in O:
                    if p < nodes[key].priority:
                        nodes[key].priority = p
                        nodes[key].backpointer = O[0]
                else:
                    nodes[key].priority = p
                    nodes[key].backpointer = O[0]
                    O.append(nodes[key])
        O.pop(0)

        if not O:
            print("Dead end")
        else:
            O.sort(key = lambda x: x.priority)

    path = backtrack(O[0], n_start)

    return path

# 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("N", [5,6]) @pytest.mark.parametrize("keepOutFov", [20]) @pytest.mark.parametrize("keepInFov", [70]) @pytest.mark.parametrize("costFcnType", [0,1]) @pytest.mark.parametrize("accuracy", [1e-12]) def test_constrainedAttitudeManeuver(show_plots, N, keepOutFov, keepInFov, costFcnType, accuracy): r""" **Validation Test Description** This unit test script tests the correctness of the path computed by the ConstrainedAttitudeManeuver module. The module is tested against Python scripts that mirror the same functions contained in the module. Tests are run for different grid coarseness levels, different keep-out fields of view, and one keep-in field of view. **Test Parameters** Args: N (int) : grid coarseness; keepOutFov (float) : Field of View (in radiants) of the keep-out boresight; keepInFov (float) : Field of View (in radiants) of the keep-in boresight; costFcnType (int) : 0 for the minimum MRP cartesian distance graph search, 1 for the effort-based graph search. accuracy (float): absolute accuracy value used in the validation tests **Description of Variables Being Tested** The tests to show the correctness of the module are the following: - First of all, an equivalent grid is built in python. The first test consists in comparing the nodes generated in Python versus the nodes generated in C++. The check consists in verifying whether the same key indices :math:`(i,j,k)` generate the same node coordinates :math:`\sigma_{BN}`. Secondly, it is checked whether the same node is constraint-compliant or -incompliant both in Python and in C++. - After running the graph-search algorithm, a check is conduced to ensure the equivalence of the computed paths. Note that this unit test does not run the effort-based version of A*, due to the slow nature of the Python implementation. If the user wishes, it is possible to uncomment line 429 to also test the effort-based graph-search algorithm. - The interpolated trajectory obtained in Python is checked versus the interpolated trajectory obtained in C++. The Python code uses the BSK-native :ref:`BSpline` library, which has its own unit test. - Lastly, a check is run on the norm of the required control torque for each time step of the interpolated trajectory. The correctness of this check should imply the correctness of the functions used in the effort-based graph-search algorithm as well. """ # each test method requires a single assert method to be called [testResults, testMessage] = CAMTestFunction(N, keepOutFov, keepInFov, costFcnType, accuracy) assert testResults < 1, testMessage
def CAMTestFunction(N, keepOutFov, keepInFov, costFcnType, accuracy): 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) Inertia = [0.02 / 3, 0., 0., 0., 0.1256 / 3, 0., 0., 0., 0.1256 / 3] InertiaTensor = unitTestSupport.np2EigenMatrix3d(Inertia) PlanetInertialPosition = np.array([10, 0, 0]) SCInertialPosition = np.array([1, 0, 0]) SCInitialAttitude = np.array([0, 0, -0.5]) SCTargetAttitude = np.array([0, 0.5, 0]) SCInitialAngRate = np.array([0, 0, 0]) SCTargetAngRate = np.array([0, 0, 0]) SCAvgAngRate = 0.03 keepOutBoresight_B = [[1, 0, 0]] keepInBoresight_B = [[0, 1, 0], [0, 0, 1]] # convert Fov angles to radiants keepOutFov = keepOutFov * macros.D2R keepInFov = keepInFov * macros.D2R constraints = {'keepOut' : [], 'keepIn' : []} constraints['keepOut'].append( constraint(PlanetInertialPosition-SCInertialPosition, 'r') ) constraints['keepIn'].append( constraint(PlanetInertialPosition-SCInertialPosition, 'g') ) data = {'keepOut_b' : keepOutBoresight_B, 'keepOut_fov' : [keepOutFov], 'keepIn_b' : keepInBoresight_B, 'keepIn_fov' : [keepInFov, keepInFov]} n_start = node(SCInitialAttitude, constraints, **data) n_goal = node(SCTargetAttitude, constraints, **data) nodes = generateGrid(n_start, n_goal, N, constraints, data) if costFcnType == 0: path = AStar(nodes, n_start, n_goal) else: path = effortBasedAStar(nodes, n_start, n_goal, SCInitialAngRate, SCTargetAngRate, SCAvgAngRate, InertiaTensor) Input = pathHandle(path, SCAvgAngRate) Output = spline(Input, SCInitialAngRate, SCTargetAngRate) pathCost = effortEvaluation(Output, InertiaTensor) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread simulationTime = macros.min2nano(2) testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) testModule = constrainedAttitudeManeuver.ConstrainedAttitudeManeuver(N) testModule.sigma_BN_goal = SCTargetAttitude testModule.omega_BN_B_goal = SCTargetAngRate testModule.avgOmega = SCAvgAngRate testModule.BSplineType = 0 testModule.costFcnType = costFcnType testModule.appendKeepOutDirection(keepOutBoresight_B[0], keepOutFov) testModule.appendKeepInDirection(keepInBoresight_B[0], keepInFov) testModule.appendKeepInDirection(keepInBoresight_B[1], keepInFov) testModule.ModelTag = "testModule" unitTestSim.AddModelToTask(unitTaskName, testModule) # connect messages SCStatesMsgData = messaging.SCStatesMsgPayload() SCStatesMsgData.r_BN_N = SCInertialPosition SCStatesMsgData.sigma_BN = SCInitialAttitude SCStatesMsgData.omega_BN_B = SCInitialAngRate SCStatesMsg = messaging.SCStatesMsg().write(SCStatesMsgData) VehicleConfigMsgData = messaging.VehicleConfigMsgPayload() VehicleConfigMsgData.ISCPntB_B = Inertia VehicleConfigMsg = messaging.VehicleConfigMsg().write(VehicleConfigMsgData) PlanetStateMsgData = messaging.SpicePlanetStateMsgPayload() PlanetStateMsgData.PositionVector = PlanetInertialPosition PlanetStateMsg = messaging.SpicePlanetStateMsg().write(PlanetStateMsgData) testModule.scStateInMsg.subscribeTo(SCStatesMsg) testModule.vehicleConfigInMsg.subscribeTo(VehicleConfigMsg) testModule.keepOutCelBodyInMsg.subscribeTo(PlanetStateMsg) testModule.keepInCelBodyInMsg.subscribeTo(PlanetStateMsg) numDataPoints = 200 samplingTime = unitTestSupport.samplingTime(simulationTime, testProcessRate, numDataPoints) CAMLog = testModule.attRefOutMsg.recorder(samplingTime) unitTestSim.AddModelToTask(unitTaskName, CAMLog) CAMLogC = testModule.attRefOutMsgC.recorder(samplingTime) unitTestSim.AddModelToTask(unitTaskName, CAMLogC) # Need to call the self-init and cross-init methods unitTestSim.InitializeSimulation() # Set the simulation time. # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. unitTestSim.ConfigureStopTime(simulationTime) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() timeData = CAMLog.times() * macros.NANO2SEC print(timeData) # print(len(CAMLog.sigma_RN)) # check correctness of grid points: for i in range(-N,N+1): for j in range(-N,N+1): for k in range(-N,N+1): if (i, j, k) in nodes: sigma_BN = nodes[(i, j, k)].sigma_BN sigma_BN_BSK = [] for p in range(3): sigma_BN_BSK.append( testModule.returnNodeCoord([i, j, k], p) ) if not unitTestSupport.isVectorEqual(sigma_BN, sigma_BN_BSK, accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Error in the coordinates of node ({},{},{}) \n".format(i, j, k)) if not nodes[(i, j, k)].isFree == testModule.returnNodeState([i, j, k]): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Error in the state of node ({},{},{}) \n".format(i, j, k)) # check that the same path is produced for p in range(len(path)): sigma_BN = path[p].sigma_BN sigma_BN_BSK = [] for j in range(3): sigma_BN_BSK.append(testModule.returnPathCoord(p,j)) if not unitTestSupport.isVectorEqual(sigma_BN, sigma_BN_BSK, accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Error in waypoint number {} in path \n".format(p)) # check interpolated path compliance for n in range(len(Output.T)): T_BSK = testModule.Output.T[n][0] sigma_BSK = np.array([testModule.Output.X1[n][0], testModule.Output.X2[n][0], testModule.Output.X3[n][0]]) sigmaDot_BSK = np.array([testModule.Output.XD1[n][0], testModule.Output.XD2[n][0], testModule.Output.XD3[n][0]]) sigmaDDot_BSK = np.array([testModule.Output.XDD1[n][0], testModule.Output.XDD2[n][0], testModule.Output.XDD3[n][0]]) T = Output.T[n][0] sigma = np.array([Output.X1[n][0], Output.X2[n][0], Output.X3[n][0]]) sigmaDot = np.array([Output.XD1[n][0], Output.XD2[n][0], Output.XD3[n][0]]) sigmaDDot = np.array([Output.XDD1[n][0], Output.XDD2[n][0], Output.XDD3[n][0]]) if not unitTestSupport.isDoubleEqual(T, T_BSK, accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Error in time at index #{} in trajectory \n".format(n)) if not unitTestSupport.isVectorEqual(sigma, sigma_BSK, accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Error in sigma at index #{} in trajectory \n".format(n)) if not unitTestSupport.isVectorEqual(sigmaDot, sigmaDot_BSK, accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Error in sigmaDot at index #{} in trajectory \n".format(n)) if not unitTestSupport.isVectorEqual(sigmaDDot, sigmaDDot_BSK, accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Error in sigmaDDot at index #{} in trajectory \n".format(n)) # check same path cost for every spline point for n in range(len(Output.T)): c1 = testModule.computeTorqueNorm(n, Inertia) sigma = [Output.X1[n][0], Output.X2[n][0], Output.X3[n][0]] sigmaDot = [Output.XD1[n][0], Output.XD2[n][0], Output.XD3[n][0]] sigmaDDot = [Output.XDD1[n][0], Output.XDD2[n][0], Output.XDD3[n][0]] c2 = np.linalg.norm(computeTorque(sigma, sigmaDot, sigmaDDot, InertiaTensor)) if not unitTestSupport.isDoubleEqual(c1, c2, accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Error torque norm in point {} in trajectory \n".format(n)) if not unitTestSupport.isDoubleEqual(pathCost, testModule.pathCost, accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Error in path cost \n") # check correctness of output message for n in range(len(timeData)): t = timeData[n] sigma_RN = [Output.getStates(t,0,0), Output.getStates(t,0,1), Output.getStates(t,0,2)] sigmaDot_RN = [Output.getStates(t,1,0), Output.getStates(t,1,1), Output.getStates(t,1,2)] sigmaDDot_RN = [Output.getStates(t,2,0), Output.getStates(t,2,1), Output.getStates(t,2,2)] RN = rbk.MRP2C(sigma_RN) omega_RN_R = rbk.dMRP2Omega(sigma_RN, sigmaDot_RN) omega_RN_N = np.matmul(omega_RN_R, RN) omegaDot_RN_R = rbk.ddMRP2dOmega(sigma_RN, sigmaDot_RN, sigmaDDot_RN) omegaDot_RN_N = np.matmul(omegaDot_RN_R, RN) if not unitTestSupport.isVectorEqual(sigma_RN, CAMLog.sigma_RN[n], accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Error in attitude reference message at t = {} sec \n".format(t)) if not unitTestSupport.isVectorEqual(omega_RN_N, CAMLog.omega_RN_N[n], accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Error in attitude reference message at t = {} sec \n".format(t)) if not unitTestSupport.isVectorEqual(omegaDot_RN_N, CAMLog.domega_RN_N[n], accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Error in attitude reference message at t = {} sec \n".format(t)) if not unitTestSupport.isVectorEqual(sigma_RN, CAMLogC.sigma_RN[n], accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Error in C attitude reference message at t = {} sec \n".format(t)) if not unitTestSupport.isVectorEqual(omega_RN_N, CAMLogC.omega_RN_N[n], accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Error in C attitude reference message at t = {} sec \n".format(t)) if not unitTestSupport.isVectorEqual(omegaDot_RN_N, CAMLogC.domega_RN_N[n], accuracy): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Error in C attitude reference message at t = {} sec \n".format(t)) return [testFailCount, ''.join(testMessages)] # # This statement below ensures that the unitTestScript can be run as a # stand-along python script # if __name__ == "__main__": CAMTestFunction( 5, # grid coarsness N 20, # keepOutFov 70, # keepInFov 1, # costFcnType 1e-12 # accuracy )