Source code for test_prescribedRotation1DOF

#
#  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:        PrescribedRotation1DOF
#   Author:             Leah Kiner
#   Last Updated:       March 28, 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 prescribedRotation1DOF
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("thetaInit", [0.0, macros.D2R * -5.0]) # [rad] @pytest.mark.parametrize("thetaRef1", [0.0, macros.D2R * -10.0]) # [rad] @pytest.mark.parametrize("thetaRef2", [macros.D2R * 5.0]) # [rad] @pytest.mark.parametrize("thetaDDotMax", [macros.D2R * 0.05, macros.D2R * 0.1]) # [rad/s^2] @pytest.mark.parametrize("accuracy", [1e-8]) def test_prescribedRotation1DOF(show_plots, coastOptionBangDuration, smoothingDuration, thetaInit, thetaRef1, thetaRef2, thetaDDotMax, accuracy): r""" **Validation Test Description** The unit test for this module ensures that the profiled 1 DOF rotation for a secondary rigid body relative to the spacecraft hub is properly computed for several different simulation configurations. The unit test profiles two successive rotations to ensure the module is correctly configured. The initial spinning body angle relative to the spacecraft hub is varied, along with the two final reference angles and the maximum angular acceleration for the rotation. This unit test also tests four different methods of profiling the rotation. Two profilers prescribe a pure bang-bang or bang-coast-bang angular acceleration profile for the rotation. The bang-bang option results in the fastest possible rotation; 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 spinning 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 (Variable must be nonzero to toggle the smoothed profiler options) thetaInit (float): [rad] Initial spinning body angle relative to the mount frame thetaRef1 (float): [rad] First spinning body reference angle relative to the mount frame thetaRef2 (float): [rad] Second spinning body reference angle relative to the mount frame thetaDDotMax (float): [rad/s^2] Maximum angular acceleration for the rotation accuracy (float): Absolute accuracy value used in the validation tests **Description of Variables Being Tested** To verify the module functionality, the final angle at the end of each rotation is checked to match the specified reference angle (``thetaRef1`` and ``thetaRef2``). Additionally, for the smoothed profiler options, the numerical derivative of the profiled angles and their rates is determined across the entire simulation in this test script. These numerical derivatives are checked with the profiled angular accelerations and angle rates to ensure the profiled acceleration is correctly integrated in the module to obtain the angles and their rates. """ 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 prescribedRotation1DOF module to be tested rotAxis_M = np.array([1.0, 0.0, 0.0]) # Spinning body rotation axis prescribedRot1DOF = prescribedRotation1DOF.PrescribedRotation1DOF() prescribedRot1DOF.ModelTag = "prescribedRotation1DOF" prescribedRot1DOF.setCoastOptionBangDuration(coastOptionBangDuration) prescribedRot1DOF.setRotHat_M(rotAxis_M) prescribedRot1DOF.setSmoothingDuration(smoothingDuration) prescribedRot1DOF.setThetaDDotMax(thetaDDotMax) prescribedRot1DOF.setThetaInit(thetaInit) unitTestSim.AddModelToTask(unitTaskName, prescribedRot1DOF) # Create the reference angle input message for the first rotation hingedRigidBodyMessageData = messaging.HingedRigidBodyMsgPayload() hingedRigidBodyMessageData.theta = thetaRef1 # [rad] hingedRigidBodyMessageData.thetaDot = 0.0 # [rad/s] hingedRigidBodyMessage = messaging.HingedRigidBodyMsg().write(hingedRigidBodyMessageData) prescribedRot1DOF.spinningBodyInMsg.subscribeTo(hingedRigidBodyMessage) # Log module data for module unit test validation prescribedRotStatesDataLog = prescribedRot1DOF.prescribedRotationOutMsg.recorder() scalarAngleDataLog = prescribedRot1DOF.spinningBodyOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, prescribedRotStatesDataLog) unitTestSim.AddModelToTask(unitTaskName, scalarAngleDataLog) # Execute the first spinning body rotation simTime = 5 * 60 # [s] unitTestSim.InitializeSimulation() unitTestSim.ConfigureStopTime(macros.sec2nano(simTime)) unitTestSim.ExecuteSimulation() # Create the reference angle input message for the second rotation hingedRigidBodyMessageData = messaging.HingedRigidBodyMsgPayload() hingedRigidBodyMessageData.theta = thetaRef2 # [rad] hingedRigidBodyMessageData.thetaDot = 0.0 # [rad/s] hingedRigidBodyMessage = messaging.HingedRigidBodyMsg().write(hingedRigidBodyMessageData) prescribedRot1DOF.spinningBodyInMsg.subscribeTo(hingedRigidBodyMessage) # Execute the second spinning body rotation unitTestSim.ConfigureStopTime(macros.sec2nano(2 * simTime)) unitTestSim.ExecuteSimulation() # Extract logged data timespan = macros.NANO2SEC * scalarAngleDataLog.times() # [s] omega_FM_F = macros.R2D * prescribedRotStatesDataLog.omega_FM_F # [deg/s] omegaPrime_FM_F = macros.R2D * prescribedRotStatesDataLog.omegaPrime_FM_F # [deg/s^2] sigma_FM = prescribedRotStatesDataLog.sigma_FM theta = macros.R2D * scalarAngleDataLog.theta # [deg] thetaDot = macros.R2D * scalarAngleDataLog.thetaDot # [deg/s] thetaDDot = omegaPrime_FM_F.dot(rotAxis_M) # [deg/s^2] # Unit test validation 1: Check that the profiler converges to the required final angles tf_1_index = int(round(simTime / testTimeStepSec)) + 1 thetaFinal1 = theta[tf_1_index] thetaFinal2 = theta[-1] thetaFinalList = [thetaFinal1, thetaFinal2] # [deg] thetaRefList = [macros.R2D * thetaRef1, macros.R2D * thetaRef2] # [deg] np.testing.assert_allclose(thetaRefList, thetaFinalList, atol=accuracy, verbose=True) # Unit test validation 2: Numerically check that the profiled accelerations, angle rates, and angles are correct if (smoothingDuration > 0.0): thetaDDotNumerical = [] thetaDotNumerical = [] for i in range(len(timespan) - 1): # First order forward difference thetaDDotNumerical.append((thetaDot[i+1] - thetaDot[i]) / testTimeStepSec) thetaDotNumerical.append((theta[i+1] - theta[i]) / testTimeStepSec) np.testing.assert_allclose(thetaDDot[0:-1], thetaDDotNumerical, atol=1e-2, verbose=True) np.testing.assert_allclose(thetaDot[0:-1], thetaDotNumerical, 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], thetaDDotNumerical - thetaDDot[0:-1], label=r'$\ddot{\theta}$') plt.plot(timespan[0:-1], thetaDotNumerical - thetaDot[0:-1], label=r"$\dot{\theta}$") 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 spinning body rotational states # 1A. Plot theta thetaInitPlotting = np.ones(len(timespan)) * macros.R2D * thetaInit # [deg] thetaRef1Plotting = np.ones(len(timespan)) * macros.R2D * thetaRef1 # [deg] thetaRef2Plotting = np.ones(len(timespan)) * macros.R2D * thetaRef2 # [deg] plt.figure() plt.clf() plt.plot(timespan, theta, label=r"$\theta$") plt.plot(timespan, thetaInitPlotting, '--', label=r'$\theta_{0}$') plt.plot(timespan, thetaRef1Plotting, '--', label=r'$\theta_{Ref_1}$') plt.plot(timespan, thetaRef2Plotting, '--', label=r'$\theta_{Ref_2}$') plt.title(r'Profiled Angle $\theta_{\mathcal{F}/\mathcal{M}}$', fontsize=14) plt.ylabel('(deg)', fontsize=14) plt.xlabel('Time (s)', fontsize=14) plt.legend(loc='upper right', prop={'size': 12}) plt.grid(True) # 1B. Plot thetaDot plt.figure() plt.clf() plt.plot(timespan, thetaDot, label=r"$\dot{\theta}$") plt.title(r'Profiled Angle Rate $\dot{\theta}_{\mathcal{F}/\mathcal{M}}$', fontsize=14) plt.ylabel('(deg/s)', fontsize=14) plt.xlabel('Time (s)', fontsize=14) plt.legend(loc='upper right', prop={'size': 12}) plt.grid(True) # 1C. Plot thetaDDot plt.figure() plt.clf() plt.plot(timespan, thetaDDot, label=r"$\ddot{\theta}$") plt.title(r'Profiled Angular Acceleration $\ddot{\theta}_{\mathcal{F}/\mathcal{M}}$ ', fontsize=14) plt.ylabel('(deg/s$^2$)', fontsize=14) plt.xlabel('Time (s)', fontsize=14) plt.legend(loc='upper right', prop={'size': 12}) plt.grid(True) # 2. Plot the spinning body prescribed rotational states # 2A. Plot PRV angle from sigma_FM phi_FM = [] for i in range(len(timespan)): phi_FM.append(macros.R2D * 4 * np.arctan(np.linalg.norm(sigma_FM[i, :]))) # [deg] plt.figure() plt.clf() plt.plot(timespan, phi_FM, label=r"$\Phi$") plt.title(r'Profiled PRV Angle $\Phi_{\mathcal{F}/\mathcal{M}}$', fontsize=14) plt.ylabel('(deg)', fontsize=14) plt.xlabel('Time (s)', fontsize=14) plt.legend(loc='center right', prop={'size': 14}) plt.grid(True) # 2B. Plot omega_FM_F plt.figure() plt.clf() plt.plot(timespan, omega_FM_F[:, 0], label=r'$\omega_{1}$') plt.plot(timespan, omega_FM_F[:, 1], label=r'$\omega_{2}$') plt.plot(timespan, omega_FM_F[:, 2], label=r'$\omega_{3}$') plt.title(r'Profiled Angular Velocity ${}^\mathcal{F} \omega_{\mathcal{F}/\mathcal{M}}$', fontsize=14) plt.ylabel('(deg/s)', fontsize=14) plt.xlabel('Time (s)', fontsize=14) plt.legend(loc='upper right', prop={'size': 14}) plt.grid(True) # 2C. Plot omegaPrime_FM_F plt.figure() plt.clf() plt.plot(timespan, omegaPrime_FM_F[:, 0], label=r'1') plt.plot(timespan, omegaPrime_FM_F[:, 1], label=r'2') plt.plot(timespan, omegaPrime_FM_F[:, 2], label=r'3') plt.title(r'Profiled Angular Acceleration ${}^\mathcal{F} \omega$Prime$_{\mathcal{F}/\mathcal{M}}$', fontsize=14) plt.ylabel('(deg/s$^2$)', fontsize=14) plt.xlabel('Time (s)', fontsize=14) plt.legend(loc='upper right', prop={'size': 14}) plt.grid(True) plt.show() plt.close("all")
if __name__ == "__main__": test_prescribedRotation1DOF( True, # show_plots 2.0, # [s] coastOptionBangDuration 2.0, # [s] smoothingDuration macros.D2R * -5.0, # [rad] thetaInit macros.D2R * -10.0, # [rad] thetaRef1 macros.D2R * 5.0, # [rad] thetaRef2 macros.D2R * 0.1, # [rad/s^2] thetaDDotMax 1e-8 # accuracy )