Source code for test_prescribedLinearTranslation

#
#  ISC License
#
#  Copyright (c) 2024, 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:        prescribedLinearTranslation
#   Author:             Leah Kiner
#   Last Updated:       March 18, 2024
#

import inspect
import os

import matplotlib.pyplot as plt
import numpy as np
import pytest

from Basilisk.architecture import bskLogging
from Basilisk.architecture import messaging
from Basilisk.simulation import prescribedLinearTranslation
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import macros

filename = inspect.getframeinfo(inspect.currentframe()).filename
path = os.path.dirname(os.path.abspath(filename))
bskName = 'Basilisk'
splitPath = path.split(bskName)


[docs] @pytest.mark.parametrize("coastOptionBangDuration", [0.0, 2.0]) # [s] @pytest.mark.parametrize("smoothingDuration", [0.0, 2.0]) # [s] @pytest.mark.parametrize("transPosInit", [0.0, -0.5]) # [m] @pytest.mark.parametrize("transPosRef1", [0.0, -1.0]) # [m] @pytest.mark.parametrize("transPosRef2", [0.5]) # [m] @pytest.mark.parametrize("transAccelMax", [0.01, 0.005]) # [m/s^2] @pytest.mark.parametrize("accuracy", [1e-8]) def test_prescribedLinearTranslation(show_plots, coastOptionBangDuration, smoothingDuration, transPosInit, transPosRef1, transPosRef2, transAccelMax, accuracy): r""" **Validation Test Description** The unit test for this module ensures that the profiled linear translation for a secondary rigid body relative to the spacecraft hub is properly computed for several different simulation configurations. This unit test profiles two successive translations to ensure the module is correctly configured. The secondary body's initial scalar translational position relative to the spacecraft hub is varied, along with the two final reference positions and the maximum translational acceleration. This unit test also tests four different methods of profiling the translation. Two profilers prescribe a pure bang-bang or bang-coast-bang linear acceleration profile for the translation. The bang-bang option results in the fastest possible translation; while the bang-coast-bang option includes a coast period with zero acceleration between the acceleration segments. The other two profilers apply smoothing to the bang-bang and bang-coast-bang acceleration profiles so that the secondary body hub-relative rates start and end at zero. **Test Parameters** Args: show_plots (bool): Variable for choosing whether plots should be displayed coastOptionBangDuration: (float): [s] Time the acceleration is applied during the bang segments (Variable must be nonzero to select the bang-coast-bang option) smoothingDuration (float) [s] Time the acceleration is smoothed to the given maximum acceleration value transPosInit (float): [m] Initial translational body position from M to F frame origin along transAxis_M transPosRef1 (float): [m] First reference position from M to F frame origin along transAxis_M transPosRef2 (float): [m] Second reference position from M to F frame origin along transAxis_M transAccelMax (float): [m/s^2] Maximum translational acceleration accuracy (float): Absolute accuracy value used in the validation tests **Description of Variables Being Tested** To verify the module functionality, the final position at the end of each translation segment is checked to match the specified reference position (``transPosRef1`` and ``transPosRef2``). Additionally, for the smoothed profiler options, the numerical derivative of the profiled displacements and velocities is determined across the entire simulation in this test script. These numerical derivatives are checked with the module's acceleration and velocity profiles to ensure the profiled acceleration is correctly integrated in the module to obtain the displacements and velocities. """ unitTaskName = "unitTask" unitProcessName = "TestProcess" bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() testTimeStepSec = 0.1 # [s] testProcessRate = macros.sec2nano(testTimeStepSec) testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Create an instance of the prescribedLinearTranslation module to be tested transAxis_M = np.array([1.0, 0.0, 0.0]) # Axis of translation prescribedTrans = prescribedLinearTranslation.PrescribedLinearTranslation() prescribedTrans.ModelTag = "prescribedTrans" prescribedTrans.setCoastOptionBangDuration(coastOptionBangDuration) prescribedTrans.setSmoothingDuration(smoothingDuration) prescribedTrans.setTransHat_M(transAxis_M) prescribedTrans.setTransAccelMax(transAccelMax) prescribedTrans.setTransPosInit(transPosInit) unitTestSim.AddModelToTask(unitTaskName, prescribedTrans) # Create the reference position input message for the first translation linearTransRigidBodyMessageData = messaging.LinearTranslationRigidBodyMsgPayload() linearTransRigidBodyMessageData.rho = transPosRef1 # [m] linearTransRigidBodyMessageData.rhoDot = 0.0 # [m/s] linearTransRigidBodyMessage = messaging.LinearTranslationRigidBodyMsg().write(linearTransRigidBodyMessageData) prescribedTrans.linearTranslationRigidBodyInMsg.subscribeTo(linearTransRigidBodyMessage) # Log module data for module unit test validation prescribedStatesDataLog = prescribedTrans.prescribedTranslationOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, prescribedStatesDataLog) # Execute the first translation segment simTime = 5 * 60 # [s] unitTestSim.InitializeSimulation() unitTestSim.ConfigureStopTime(macros.sec2nano(simTime)) unitTestSim.ExecuteSimulation() # Create the reference position input message for the second translation linearTransRigidBodyMessageData = messaging.LinearTranslationRigidBodyMsgPayload() linearTransRigidBodyMessageData.rho = transPosRef2 # [m] linearTransRigidBodyMessageData.rhoDot = 0.0 # [m/s] linearTransRigidBodyMessage = messaging.LinearTranslationRigidBodyMsg().write(linearTransRigidBodyMessageData) prescribedTrans.linearTranslationRigidBodyInMsg.subscribeTo(linearTransRigidBodyMessage) # Execute the second translation segment unitTestSim.ConfigureStopTime(macros.sec2nano(2 * simTime)) unitTestSim.ExecuteSimulation() # Extract the logged data for plotting and data comparison timespan = macros.NANO2SEC * prescribedStatesDataLog.times() # [s] r_FM_M = prescribedStatesDataLog.r_FM_M # [m] rPrime_FM_M = prescribedStatesDataLog.rPrime_FM_M # [m/s] rPrimePrime_FM_M = prescribedStatesDataLog.rPrimePrime_FM_M # [m/s^2] # Unit test validation 1: Check that the profiler converges to the required final positions tf_1_index = int(round(simTime / testTimeStepSec)) + 1 transPosFinal1 = r_FM_M[tf_1_index].dot(transAxis_M) transPosFinal2 = r_FM_M[-1].dot(transAxis_M) transPosFinalList = [transPosFinal1, transPosFinal2] # [m] transPosRefList = [transPosRef1, transPosRef2] # [m] np.testing.assert_allclose(transPosRefList, transPosFinalList, atol=accuracy, verbose=True) # Unit test validation 2: Numerically check that the profiled accelerations, # velocities, and displacements are correct if (smoothingDuration > 0.0): transAccel = rPrimePrime_FM_M.dot(transAxis_M) transVel = rPrime_FM_M.dot(transAxis_M) transPos = r_FM_M.dot(transAxis_M) transAccelNumerical = [] transVelNumerical = [] for i in range(len(timespan) - 1): # First order forward difference transAccelNumerical.append((transVel[i+1] - transVel[i]) / testTimeStepSec) transVelNumerical.append((transPos[i+1] - transPos[i]) / testTimeStepSec) np.testing.assert_allclose(transAccel[0:-1], transAccelNumerical, atol=1e-2, verbose=True) np.testing.assert_allclose(transVel[0:-1], transVelNumerical, atol=1e-2, verbose=True) if show_plots: # Plot the difference between the numerical and profiled results plt.figure() plt.clf() plt.plot(timespan[0:-1], transAccelNumerical - transAccel[0:-1], label=r'$\ddot{\rho}$') plt.plot(timespan[0:-1], transVelNumerical - transVel[0:-1], label=r"$\dot{\rho}$") plt.title(r'Profiled vs Numerical Difference', fontsize=14) plt.legend(loc='upper right', prop={'size': 12}) plt.grid(True) if show_plots: # 1. Plot the scalar translational states # 1A. Plot transPos transPosInitPlotting = np.ones(len(timespan)) * transPosInit transPosRef1Plotting = np.ones(len(timespan)) * transPosRef1 transPosRef2Plotting = np.ones(len(timespan)) * transPosRef2 plt.figure() plt.clf() plt.plot(timespan, r_FM_M.dot(transAxis_M), label=r"$l$") plt.plot(timespan, transPosInitPlotting, '--', label=r'$\rho_{0}$') plt.plot(timespan, transPosRef1Plotting, '--', label=r'$\rho_{Ref_1}$') plt.plot(timespan, transPosRef2Plotting, '--', label=r'$\rho_{Ref_2}$') plt.title(r'Profiled Translational Position $\rho_{\mathcal{F}/\mathcal{M}}$', fontsize=14) plt.ylabel('(m)', fontsize=14) plt.xlabel('Time (s)', fontsize=14) plt.legend(loc='upper right', prop={'size': 12}) plt.grid(True) # 1B. Plot transVel plt.figure() plt.clf() plt.plot(timespan, rPrime_FM_M.dot(transAxis_M), label=r"$\dot{\rho}$") plt.title(r'Profiled Translational Velocity $\dot{\rho}_{\mathcal{F}/\mathcal{M}}$', fontsize=14) plt.ylabel('(m/s)', fontsize=14) plt.xlabel('Time (s)', fontsize=14) plt.legend(loc='upper right', prop={'size': 12}) plt.grid(True) # 1C. Plot transAccel plt.figure() plt.clf() plt.plot(timespan, rPrimePrime_FM_M.dot(transAxis_M), label=r"$\ddot{\rho}$") plt.title(r'Profiled Translational Acceleration $\ddot{\rho}_{\mathcal{F}/\mathcal{M}}$ ', fontsize=14) plt.ylabel('(m/s$^2$)', fontsize=14) plt.xlabel('Time (s)', fontsize=14) plt.legend(loc='upper right', prop={'size': 12}) plt.grid(True) # 2. Plot the prescribed translational states # 2A. Plot r_FM_M transPosRef1Plotting = np.ones(len(timespan)) * transPosRef1 # [m] transPosRef2Plotting = np.ones(len(timespan)) * transPosRef2 # [m] plt.figure() plt.clf() plt.plot(timespan, r_FM_M[:, 0], label=r'$r_{1}$') plt.plot(timespan, r_FM_M[:, 1], label=r'$r_{2}$') plt.plot(timespan, r_FM_M[:, 2], label=r'$r_{3}$') plt.plot(timespan, transPosRef1Plotting, '--', label=r'$\rho_{Ref_1}$') plt.plot(timespan, transPosRef2Plotting, '--', label=r'$\rho_{Ref_2}$') plt.title(r'${}^\mathcal{M} r_{\mathcal{F}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) plt.ylabel('(m)', fontsize=14) plt.xlabel('Time (s)', fontsize=14) plt.legend(loc='center left', prop={'size': 12}) plt.grid(True) # 2B. Plot rPrime_FM_F plt.figure() plt.clf() plt.plot(timespan, rPrime_FM_M[:, 0], label='1') plt.plot(timespan, rPrime_FM_M[:, 1], label='2') plt.plot(timespan, rPrime_FM_M[:, 2], label='3') plt.title(r'${}^\mathcal{M} r$Prime$_{\mathcal{F}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) plt.ylabel('(m/s)', fontsize=14) plt.xlabel('Time (s)', fontsize=14) plt.legend(loc='upper left', prop={'size': 12}) plt.grid(True) # 2C. Plot rPrimePrime_FM_F plt.figure() plt.clf() plt.plot(timespan, rPrimePrime_FM_M[:, 0], label='1') plt.plot(timespan, rPrimePrime_FM_M[:, 1], label='2') plt.plot(timespan, rPrimePrime_FM_M[:, 2], label='3') plt.title(r'${}^\mathcal{M} r$PrimePrime$_{\mathcal{F}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) plt.ylabel('(m/s$^2$)', fontsize=14) plt.xlabel('Time (s)', fontsize=14) plt.legend(loc='upper left', prop={'size': 12}) plt.grid(True) plt.show() plt.close("all")
if __name__ == "__main__": test_prescribedLinearTranslation( True, # show_plots 2.0, # [s] coastOptionBangDuration 2.0, # [s] smoothingDuration -0.5, # [m] transPosInit -1.0, # [m] transPosRef1 0.5, # [m] transPosRef2 0.01, # [m/s^2] transAccelMax 1e-8 # accuracy )