diff --git a/docs/source/Support/bskReleaseNotes.rst b/docs/source/Support/bskReleaseNotes.rst index 693dac8a22..c366c19d78 100644 --- a/docs/source/Support/bskReleaseNotes.rst +++ b/docs/source/Support/bskReleaseNotes.rst @@ -64,6 +64,8 @@ Version |release| - Added support for Vizard 2.1.6.1 - Updated :ref:`MtbEffector` to include missing swig interface file for a message definition and corrected message table in the module documentation. +- Added smoothed bang-bang and smoothed bang-coast-bang profiler options to the :ref:`prescribedLinearTranslation` + simulation module Version 2.2.1 (Dec. 22, 2023) diff --git a/src/simulation/deviceInterface/prescribedLinearTranslation/_UnitTest/test_prescribedLinearTranslation.py b/src/simulation/deviceInterface/prescribedLinearTranslation/_UnitTest/test_prescribedLinearTranslation.py index f1602d2b91..bf8e317295 100644 --- a/src/simulation/deviceInterface/prescribedLinearTranslation/_UnitTest/test_prescribedLinearTranslation.py +++ b/src/simulation/deviceInterface/prescribedLinearTranslation/_UnitTest/test_prescribedLinearTranslation.py @@ -19,8 +19,8 @@ # # Unit Test Script # Module Name: prescribedLinearTranslation -# Author: Patrick Kenneally and Leah Kiner -# Creation Date: Feb 12, 2024 +# Author: Leah Kiner +# Last Updated: March 18, 2024 # import inspect @@ -29,6 +29,7 @@ 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 @@ -41,14 +42,16 @@ splitPath = path.split(bskName) -@pytest.mark.parametrize("coastOptionRampDuration", [0.0, 2.0, 5.0]) # [s] -@pytest.mark.parametrize("transPosInit", [0, -0.75]) # [m] -@pytest.mark.parametrize("transPosRef1", [0, -0.5]) # [m] -@pytest.mark.parametrize("transPosRef2", [-0.75, 1.0]) # [m] +@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-4]) +@pytest.mark.parametrize("accuracy", [1e-8]) def test_prescribedLinearTranslation(show_plots, - coastOptionRampDuration, + coastOptionBangDuration, + smoothingDuration, transPosInit, transPosRef1, transPosRef2, @@ -57,31 +60,39 @@ def test_prescribedLinearTranslation(show_plots, r""" **Validation Test Description** - This unit test ensures that a profiled 1 DOF translation for a secondary rigid body connected to a 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 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 both methods of profiling the translation, where either a pure bang-bang - acceleration profile can be selected for the translation, or a coast option can be selected where the accelerations - are only applied for a specified ramp time and a coast segment with zero acceleration is applied between the two - acceleration periods. To validate the module, the final hub-relative position at the end of each translation is - checked to match the specified reference position. + 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 - coastOptionRampDuration: (double): [s] Ramp duration used for the coast option - transPosInit (float): [m] Initial translational body position from M to F frame origin along transHat_M - transPosRef1 (float): [m] First reference position from M to F frame origin along transHat_M - transPosRef2 (float): [m] Second reference position from M to F frame origin along transHat_M + 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** - This unit test checks that the final translational body position at the end of each translation converge to the - specified reference values ``transPosRef1`` and ``transPosRef2``. + 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" @@ -91,107 +102,48 @@ def test_prescribedLinearTranslation(show_plots, # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() - # Create the test thread - testTimeStepSec = 0.01 # [s] + 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" - transHat_M = np.array([0.5, 0.0, 0.5 * np.sqrt(3)]) - prescribedTrans.setCoastOptionRampDuration(coastOptionRampDuration) - prescribedTrans.setTransHat_M(transHat_M) - prescribedTrans.setTransAccelMax(transAccelMax) # [m/s^2] - prescribedTrans.setTransPosInit(transPosInit) # [m] - - # Add the prescribedTrans test module to runtime call list + prescribedTrans.setCoastOptionBangDuration(coastOptionBangDuration) + prescribedTrans.setSmoothingDuration(smoothingDuration) + prescribedTrans.setTransHat_M(transAxis_M) + prescribedTrans.setTransAccelMax(transAccelMax) + prescribedTrans.setTransPosInit(transPosInit) unitTestSim.AddModelToTask(unitTaskName, prescribedTrans) - # Create the input reference position message for the first translation - linearTranslationRigidBodyMessageData = messaging.LinearTranslationRigidBodyMsgPayload() - linearTranslationRigidBodyMessageData.rho = transPosRef1 - linearTranslationRigidBodyMessageData.rhoDot = 0.0 - linearTranslationRigidBodyMessage = messaging.LinearTranslationRigidBodyMsg().write(linearTranslationRigidBodyMessageData) - prescribedTrans.linearTranslationRigidBodyInMsg.subscribeTo(linearTranslationRigidBodyMessage) + # 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) - # Initialize the simulation + # Execute the first translation segment + simTime = 5 * 60 # [s] unitTestSim.InitializeSimulation() - - # Determine the required simulation time for the first translation - transVelInit = 0.0 # [m/s] - tCoast_1 = 0.0 # [s] - if coastOptionRampDuration > 0.0: - # Determine the position and velocity at the end of the ramp segment/start of the coast segment - if transPosInit < transPosRef1: - transPos_tr_1 = ((0.5 * transAccelMax * coastOptionRampDuration * coastOptionRampDuration) - + (transVelInit * coastOptionRampDuration) - + transPosInit) # [m] - transVel_tr_1 = transAccelMax * coastOptionRampDuration + transVelInit # [m/s] - else: - transPos_tr_1 = (- ((0.5 * transAccelMax * coastOptionRampDuration * coastOptionRampDuration) - + (transVelInit * coastOptionRampDuration)) - + transPosInit) # [m] - transVel_tr_1 = - transAccelMax * coastOptionRampDuration + transVelInit # [m/s] - - # Determine the distance traveled during the coast period - deltaPosCoast_1 = transPosRef1 - transPosInit - 2 * (transPos_tr_1 - transPosInit) # [m] - - # Determine the time duration of the coast segment - tCoast_1 = np.abs(deltaPosCoast_1) / np.abs(transVel_tr_1) # [s] - translation1ReqTime = (2 * coastOptionRampDuration) + tCoast_1 # [s] - else: - translation1ReqTime = np.sqrt(((0.5 * np.abs(transPosRef1 - transPosInit)) * 8) / transAccelMax) + 5 # [s] - - translation1ExtraTime = 5 # [s] - unitTestSim.ConfigureStopTime(macros.sec2nano(translation1ReqTime + translation1ExtraTime)) - - # Execute the first translation + unitTestSim.ConfigureStopTime(macros.sec2nano(simTime)) unitTestSim.ExecuteSimulation() - # Create the input reference position message for the second translation - linearTranslationRigidBodyMessageData = messaging.LinearTranslationRigidBodyMsgPayload() - linearTranslationRigidBodyMessageData.rho = transPosRef2 - linearTranslationRigidBodyMessageData.rhoDot = 0.0 - linearTranslationRigidBodyMessage = messaging.LinearTranslationRigidBodyMsg().write(linearTranslationRigidBodyMessageData) - prescribedTrans.linearTranslationRigidBodyInMsg.subscribeTo(linearTranslationRigidBodyMessage) - - # Determine the required simulation time for the second translation - tCoast_2 = 0.0 # [s] - if coastOptionRampDuration > 0.0: - # Determine the position and velocity at the end of the ramp segment/start of the coast segment - if transPosRef1 < transPosRef2: - transPos_tr_2 = ((0.5 * transAccelMax * coastOptionRampDuration * coastOptionRampDuration) - + (transVelInit * coastOptionRampDuration) - + transPosRef1) # [m] - transVel_tr_2 = transAccelMax * coastOptionRampDuration + transVelInit # [m/s] - else: - transPos_tr_2 = (- ((0.5 * transAccelMax * coastOptionRampDuration * coastOptionRampDuration) - + (transVelInit * coastOptionRampDuration)) - + transPosRef1) # [m] - transVel_tr_2 = - transAccelMax * coastOptionRampDuration + transVelInit # [m/s] - - # Determine the distance traveled during the coast period - deltaPosCoast_2 = transPosRef2 - transPosRef1 - 2 * (transPos_tr_2 - transPosRef1) # [m] - - # Determine the time duration of the coast segment - tCoast_2 = np.abs(deltaPosCoast_2) / np.abs(transVel_tr_2) # [s] - translation2ReqTime = (2 * coastOptionRampDuration) + tCoast_2 # [s] - else: - translation2ReqTime = np.sqrt(((0.5 * np.abs(transPosRef2 - transPosRef1)) * 8) / transAccelMax) + 5 # [s] - - translation2ExtraTime = 5 # [s] - unitTestSim.ConfigureStopTime(macros.sec2nano(translation1ReqTime - + translation1ExtraTime - + translation2ReqTime - + translation2ExtraTime)) - - # Execute the second translation + # 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 @@ -200,51 +152,49 @@ def test_prescribedLinearTranslation(show_plots, rPrime_FM_M = prescribedStatesDataLog.rPrime_FM_M # [m/s] rPrimePrime_FM_M = prescribedStatesDataLog.rPrimePrime_FM_M # [m/s^2] - # Unit test validation - # Store the truth data used to validate the module in two lists - if coastOptionRampDuration > 0.0: - # Compute tf for the first translation, and tInit tf for the second translation - tf_1 = 2 * coastOptionRampDuration + tCoast_1 # [s] - tInit_2 = translation1ReqTime + translation1ExtraTime # [s] - tf_2 = tInit_2 + (2 * coastOptionRampDuration) + tCoast_2 # [s] - - # Compute the timespan indices for each check - tf_1_index = int(round(tf_1 / testTimeStepSec)) + 1 - tInit_2_index = int(round(tInit_2 / testTimeStepSec)) + 1 - tf_2_index = int(round(tf_2 / testTimeStepSec)) + 1 - - # Store the timespan indices in a list - timeCheckIndicesList = [tf_1_index, tInit_2_index, tf_2_index] - - # Store the positions to check in a list - transPosCheckList = [transPosRef1, transPosRef1, transPosRef2] - - else: - # Compute tf for the first translation, and tInit tf for the second translation - tf_1 = translation1ReqTime # [s] - tInit_2 = translation1ReqTime + translation1ExtraTime # [s] - tf_2 = tInit_2 + translation2ReqTime # [s] - - # Compute the timespan indices for each check - tf_1_index = int(round(tf_1 / testTimeStepSec)) + 1 - tInit_2_index = int(round(tInit_2 / testTimeStepSec)) + 1 - tf_2_index = int(round(tf_2 / testTimeStepSec)) + 1 - - # Store the timespan indices in a list - timeCheckIndicesList = [tf_1_index, tInit_2_index, tf_2_index] - - # Store the positions to check in a list - transPosCheckList = [transPosRef1, transPosRef1, transPosRef2] - - # Use the two truth data lists to compare with the module-extracted data - np.testing.assert_allclose(r_FM_M.dot(transHat_M)[timeCheckIndicesList], - transPosCheckList, + # 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) - if show_plots: - plt.close("all") # clears out plots from earlier test runs + # 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 @@ -252,11 +202,11 @@ def test_prescribedLinearTranslation(show_plots, transPosRef2Plotting = np.ones(len(timespan)) * transPosRef2 plt.figure() plt.clf() - plt.plot(timespan, r_FM_M.dot(transHat_M), label=r"$l$") + 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'Translational Position $\rho_{\mathcal{F}/\mathcal{M}}$', fontsize=14) + 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}) @@ -265,8 +215,8 @@ def test_prescribedLinearTranslation(show_plots, # 1B. Plot transVel plt.figure() plt.clf() - plt.plot(timespan, rPrime_FM_M.dot(transHat_M), label=r"$\dot{\rho}$") - plt.title(r'Translational Velocity $\dot{\rho}_{\mathcal{F}/\mathcal{M}}$', fontsize=14) + 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}) @@ -275,8 +225,8 @@ def test_prescribedLinearTranslation(show_plots, # 1C. Plot transAccel plt.figure() plt.clf() - plt.plot(timespan, rPrimePrime_FM_M.dot(transHat_M), label=r"$\ddot{\rho}$") - plt.title(r'Translational Acceleration $\ddot{\rho}_{\mathcal{F}/\mathcal{M}}$ ', fontsize=14) + 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}) @@ -284,32 +234,22 @@ def test_prescribedLinearTranslation(show_plots, # 2. Plot the prescribed translational states # 2A. Plot r_FM_M - r_FM_M_Ref1 = transPosRef1 * transHat_M - r_FM_M_1_Ref1 = np.ones(len(timespan[0:timeCheckIndicesList[0]])) * r_FM_M_Ref1[0] - r_FM_M_2_Ref1 = np.ones(len(timespan[0:timeCheckIndicesList[0]])) * r_FM_M_Ref1[1] - r_FM_M_3_Ref1 = np.ones(len(timespan[0:timeCheckIndicesList[0]])) * r_FM_M_Ref1[2] - r_FM_M_Ref2 = transPosRef2 * transHat_M - r_FM_M_1_Ref2 = np.ones(len(timespan[timeCheckIndicesList[0]:-1])) * r_FM_M_Ref2[0] - r_FM_M_2_Ref2 = np.ones(len(timespan[timeCheckIndicesList[0]:-1])) * r_FM_M_Ref2[1] - r_FM_M_3_Ref2 = np.ones(len(timespan[timeCheckIndicesList[0]:-1])) * r_FM_M_Ref2[2] + 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[0:timeCheckIndicesList[0]], r_FM_M_1_Ref1, '--', label=r'$r_{1 Ref_{1}}$') - plt.plot(timespan[0:timeCheckIndicesList[0]], r_FM_M_2_Ref1, '--', label=r'$r_{2 Ref_{1}}$') - plt.plot(timespan[0:timeCheckIndicesList[0]], r_FM_M_3_Ref1, '--', label=r'$r_{3 Ref_{1}}$') - plt.plot(timespan[timeCheckIndicesList[0]:-1], r_FM_M_1_Ref2, '--', label=r'$r_{1 Ref_{2}}$') - plt.plot(timespan[timeCheckIndicesList[0]:-1], r_FM_M_2_Ref2, '--', label=r'$r_{2 Ref_{2}}$') - plt.plot(timespan[timeCheckIndicesList[0]:-1], r_FM_M_3_Ref2, '--', label=r'$r_{3 Ref_{2}}$') + 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) - # Plot rPrime_FM_F + # 2B. Plot rPrime_FM_F plt.figure() plt.clf() plt.plot(timespan, rPrime_FM_M[:, 0], label='1') @@ -321,17 +261,30 @@ def test_prescribedLinearTranslation(show_plots, plt.legend(loc='upper left', prop={'size': 12}) plt.grid(True) - plt.show() - plt.close("all") + # 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, - 0.0, # [m] transPosInit - -0.5, # [m] transPosRef1 - 1.0, # [m] transPosRef2 - 0.01, # [m/s^2] transAccelMax - 1e-4 # accuracy - ) + 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 + ) \ No newline at end of file diff --git a/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp b/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp index 9b66cc3dda..6f443164b6 100755 --- a/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp +++ b/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp @@ -26,254 +26,601 @@ /*! This method self initializes the C-wrapped output message. @return void */ -void PrescribedLinearTranslation::SelfInit() -{ +void PrescribedLinearTranslation::SelfInit() { PrescribedTranslationMsg_C_init(&this->prescribedTranslationOutMsgC); } - -/*! This method performs a complete reset of the module. The input messages are checked to ensure they are linked. +/*! This method resets required module variables and checks the input messages to ensure they are linked. @return void - @param callTime [ns] Simulation time the method is called + @param callTime [ns] Time the method is called */ -void PrescribedLinearTranslation::Reset(uint64_t callTime) -{ +void PrescribedLinearTranslation::Reset(uint64_t callTime) { if (!this->linearTranslationRigidBodyInMsg.isLinked()) { - _bskLog(this->bskLogger, BSK_ERROR, "Error: prescribedLinearTranslation.linearTranslationRigidBodyInMsg wasn't connected."); + _bskLog(this->bskLogger, + BSK_ERROR, + "prescribedLinearTranslation.linearTranslationRigidBodyInMsg wasn't connected."); } - // Set the initial time this->tInit = 0.0; - this->transPos = this->transPosInit; this->transVel = 0.0; - // Set the initial convergence to true to enter the correct loop in the Update() method on the first pass + // Set the initial convergence to true to enter the required loop in Update() method on the first pass this->convergence = true; } -/*! This method profiles the prescribed trajectory and updates the prescribed states as a function of time. -The prescribed states are then written to the output message. +/*! This method profiles the translation and updates the prescribed translational states as a function of time. +The prescribed translational states are then written to the output message. @return void - @param callTime [ns] Simulation time the method is called + @param callTime [ns] Time the method is called */ -void PrescribedLinearTranslation::UpdateState(uint64_t callTime) -{ +void PrescribedLinearTranslation::UpdateState(uint64_t callTime) { + // Read the input message LinearTranslationRigidBodyMsgPayload linearTranslationRigidBodyIn; - PrescribedTranslationMsgPayload prescribedTranslationMsgOut; - - prescribedTranslationMsgOut = PrescribedTranslationMsgPayload(); - - linearTranslationRigidBodyIn = LinearTranslationRigidBodyMsgPayload(); if (this->linearTranslationRigidBodyInMsg.isWritten()) { + linearTranslationRigidBodyIn = LinearTranslationRigidBodyMsgPayload(); linearTranslationRigidBodyIn = this->linearTranslationRigidBodyInMsg(); } - // This loop is entered (a) initially and (b) when each translation is complete. - // The parameters used to profile the translation are updated in this statement. + /* This loop is entered (a) initially and (b) when each rotation is complete. + The parameters used to profile the translation are updated in this statement. */ if (this->linearTranslationRigidBodyInMsg.timeWritten() <= callTime && this->convergence) { // Update the initial time as the current simulation time this->tInit = callTime * NANO2SEC; - // Store the reference scalar position - this->transPosRef = linearTranslationRigidBodyIn.rho; - + // Update the initial hub-relative position this->transPosInit = this->transPos; - // Set the convergence to false until the translation is complete - this->convergence = false; + // Store the reference position + this->transPosRef = linearTranslationRigidBodyIn.rho; // Set the parameters required to profile the translation - if (this->coastOptionRampDuration > 0.0) { - this->computeCoastParameters(); + if (this->transPosRef != this->transPosInit) { + this->computeTranslationParameters(); } else { - this->computeParametersNoCoast(); + this->t_f = this->tInit; } - } - double t = callTime * NANO2SEC; + // Set the convergence to false until the translation is complete + this->convergence = false; + } // Compute the scalar translational states at the current simulation time - if (this->coastOptionRampDuration > 0.0) { - if (this->isInFirstRampSegment(t)) { - this->computeFirstRampSegment(t); - } else if (this->isInCoastSegment(t)) { - this->computeCoastSegment(t); - } else if (this->isInSecondRampSegment(t)) { - this->computeSecondRampSegment(t); + this->computeCurrentState(callTime * NANO2SEC); + + // Write the module output messages + this->writeOutputMessages(callTime); +} + +/*! This intermediate method groups the calculation of translation parameters into a single method. + @return void +*/ +void PrescribedLinearTranslation::computeTranslationParameters() { + if (this->coastOptionBangDuration > 0.0) { + if (this->smoothingDuration > 0.0) { + this->computeSmoothedBangCoastBangParameters(); } else { - this->computeTranslationComplete(); + this->computeBangCoastBangParametersNoSmoothing(); } } else { - if (this->isInFirstRampSegmentNoCoast(t)) { - this->computeFirstRampSegment(t); - } else if (this->isInSecondRampSegmentNoCoast(t)) { - this->computeSecondRampSegment(t); + if (this->smoothingDuration > 0.0) { + this->computeSmoothedBangBangParameters(); } else { - this->computeTranslationComplete(); + this->computeBangBangParametersNoSmoothing(); } } +} - // [m] Translational body position relative to the Mount frame expressed in M frame components - Eigen::Vector3d r_FM_M = this->transPos*this->transHat_M; - - // [m/s] B frame time derivative of r_FM_M expressed in M frame components - Eigen::Vector3d rPrime_FM_M = this->transVel*this->transHat_M; +/*! This method computes the required parameters for the translation with a non-smoothed bang-bang acceleration profile. + @return void +*/ +void PrescribedLinearTranslation::computeBangBangParametersNoSmoothing() { + // Determine the total time required for the translation + double totalTransTime = sqrt(((0.5 * fabs(this->transPosRef - this->transPosInit)) * 8.0) / this->transAccelMax); - // [m/s^2] B frame time derivative of rPrime_FM_M expressed in M frame components - Eigen::Vector3d rPrimePrime_FM_M = this->transAccel*this->transHat_M; + // Determine the time when the translation is complete t_f + this->t_f = this->tInit + totalTransTime; - // Write the output message - eigenVector3d2CArray(r_FM_M, prescribedTranslationMsgOut.r_FM_M); - eigenVector3d2CArray(rPrime_FM_M, prescribedTranslationMsgOut.rPrime_FM_M); - eigenVector3d2CArray(rPrimePrime_FM_M, prescribedTranslationMsgOut.rPrimePrime_FM_M); + // Determine the time halfway through the translation + this->t_b1 = this->tInit + (totalTransTime / 2.0); - this->prescribedTranslationOutMsg.write(&prescribedTranslationMsgOut, this->moduleID, callTime); - PrescribedTranslationMsg_C_write(&prescribedTranslationMsgOut, &prescribedTranslationOutMsgC, this->moduleID, callTime); + // Define the parabolic constants for the first and second half of the translation + this->a = 0.5 * (this->transPosRef - this->transPosInit) + / ((this->t_b1 - this->tInit) * (this->t_b1 - this->tInit)); + this->b = -0.5 * (this->transPosRef - this->transPosInit) + / ((this->t_b1 - this->t_f) * (this->t_b1 - this->t_f)); } -/*! This method determines if the current time is within the first ramp segment for the coast option. - @return bool - @param t [s] Current simulation time +/*! This method computes the required parameters for the translation with a non-smoothed bang-coast-bang acceleration profile. + @return void */ -bool PrescribedLinearTranslation::isInFirstRampSegment(double t) const { - return (t <= this->tr && this->tf - this->tInit != 0); +void PrescribedLinearTranslation::computeBangCoastBangParametersNoSmoothing() { + double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); + + // Determine the time at the end of the first bang segment t_b1 + this->t_b1 = this->tInit + this->coastOptionBangDuration; + + // Determine the hub-relative position at time t_b1 + this->transPos_tb1 = sign * 0.5 * this->transAccelMax * this->coastOptionBangDuration + * this->coastOptionBangDuration + this->transPosInit; + this->transVel_tb1 = sign * this->transAccelMax * this->coastOptionBangDuration; + + // Determine the distance traveled during the coast period + double deltaPosCoast = this->transPosRef - this->transPosInit - 2.0 * (this->transPos_tb1 - this->transPosInit); + + // Determine the duration of the coast segment coastDuration + double coastDuration = fabs(deltaPosCoast / this->transVel_tb1); + + // Determine the time at the end of the coast segment t_c + this->t_c = this->t_b1 + coastDuration; + + // Determine the hub-relative position at time t_c + double transPos_tc = this->transPos_tb1 + deltaPosCoast; + + // Determine the time when the translation is complete t_f + this->t_f = this->t_c + this->coastOptionBangDuration; + + // Define the parabolic constants for the first and second bang segments of the translation + this->a = (this->transPos_tb1 - this->transPosInit) / ((this->t_b1 - this->tInit) * (this->t_b1 - this->tInit)); + this->b = -(this->transPosRef - transPos_tc) / ((this->t_c - this->t_f) * (this->t_c - this->t_f)); } -/*! This method determines if the current time is within the coast segment for the coast option. - @return bool - @param t [s] Current simulation time +/*! This method computes the required parameters for the translation with a smoothed bang-bang acceleration profile. + @return void */ -bool PrescribedLinearTranslation::isInCoastSegment(double t) const { - return (t > this->tr && t <= this->tc && this->tf - this->tInit != 0); +void PrescribedLinearTranslation::computeSmoothedBangBangParameters() { + double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); + + // Determine the time at the end of the first smoothing segment t_s1 + this->t_s1 = this->tInit + this->smoothingDuration; + + // Determine the hub-relative position and velocity at time t_s1 + this->transVel_ts1 = sign * 0.5 * this->transAccelMax * this->smoothingDuration; + this->transPos_ts1 = sign * (3.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transPosInit; + + // Determine the duration of the bang segment bangDuration + double aTerm = sign * 0.5 * this->transAccelMax; + double bTerm = (sign * this->transAccelMax * this->smoothingDuration + this->transVel_ts1) / aTerm; + double cTerm = (sign * (2.0 / 5.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transVel_ts1 * this->smoothingDuration + this->transPos_ts1 + - 0.5 * (this->transPosRef + this->transPosInit)) / aTerm; + double bangDuration = (- bTerm + sqrt(bTerm * bTerm - 4.0 * cTerm)) / 2.0; + + // Determine the time at the end of the first bang segment t_b1 + this->t_b1 = this->t_s1 + bangDuration; + + // Determine the hub-relative position and velocity at time t_b1 + this->transVel_tb1 = sign * this->transAccelMax * bangDuration + this->transVel_ts1; + this->transPos_tb1 = sign * 0.5 * this->transAccelMax * bangDuration * bangDuration + + this->transVel_ts1 * bangDuration + this->transPos_ts1; + + // Determine the time at the end of the second smoothing segment t_s2 + this->t_s2 = this->t_b1 + 2.0 * this->smoothingDuration; + + // Determine the hub-relative position and velocity at time t_s2 + this->transVel_ts2 = this->transVel_tb1; + this->transPos_ts2 = sign * (4.0 / 5.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transVel_tb1 * 2.0 * this->smoothingDuration + this->transPos_tb1; + + // Determine the time at the end of the second bang segment t_b2 + this->t_b2 = this->t_s2 + bangDuration; + + // Determine the hub-relative position and velocity at time t_b2 + this->transVel_tb2 = - sign * this->transAccelMax * bangDuration + this->transVel_ts2; + this->transPos_tb2 = - sign * 0.5 * this->transAccelMax * bangDuration * bangDuration + + this->transVel_ts2 * bangDuration + this->transPos_ts2; + + // Determine the time when the translation is complete t_f + this->t_f = this->t_b2 + this->smoothingDuration; } -/*! This method determines if the current time is within the second ramp segment for the coast option. - @return bool - @param t [s] Current simulation time +/*! This method computes the required parameters for the translation with a smoothed bang-coast-bang acceleration profile. + @return void */ -bool PrescribedLinearTranslation::isInSecondRampSegment(double t) const { - return (t > this->tc && t <= this->tf && this->tf - this->tInit != 0); +void PrescribedLinearTranslation::computeSmoothedBangCoastBangParameters() { + double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); + + // Determine the time at the end of the first smoothing segment t_s1 + this->t_s1 = this->tInit + this->smoothingDuration; + + // Determine the hub-relative position and velocity at time t_s1 + this->transVel_ts1 = sign * 0.5 * this->transAccelMax * this->smoothingDuration; + this->transPos_ts1 = sign * (3.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transPosInit; + + // Determine the time at the end of the first bang segment t_b1 + this->t_b1 = this->t_s1 + this->coastOptionBangDuration; + + // Determine the hub-relative position and velocity at time t_b1 + this->transVel_tb1 = sign * this->transAccelMax * this->coastOptionBangDuration + this->transVel_ts1; + this->transPos_tb1 = sign * 0.5 * this->transAccelMax * this->coastOptionBangDuration + * this->coastOptionBangDuration + this->transVel_ts1 * this->coastOptionBangDuration + + this->transPos_ts1; + + // Determine the time at the end of the second smoothing segment t_s2 + this->t_s2 = this->t_b1 + this->smoothingDuration; + + // Determine the hub-relative position and velocity at time t_s2 + this->transVel_ts2 = sign * 0.5 * this->transAccelMax * this->smoothingDuration + this->transVel_tb1; + this->transPos_ts2 = sign * (7.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transVel_tb1 * this->smoothingDuration + this->transPos_tb1; + + // Compute the time at the end of the coast segment t_c + double deltaPosCoast = (this->transPosRef - this->transPosInit) - 2 * (this->transPos_ts2 - this->transPosInit); + this->t_c = (deltaPosCoast / this->transVel_ts2) + this->t_s2; + + // Determine the hub-relative position and velocity at time t_c + this->transVel_tc = this->transVel_ts2; + this->transPos_tc = this->transVel_ts2 * (this->t_c - this->t_s2) + this->transPos_ts2; + + // Determine the time at the end of the third smoothing segment t_s3 + this->t_s3 = this->t_c + this->smoothingDuration; + + // Determine the hub-relative position and velocity at time t_s3 + this->transVel_ts3 = - sign * 0.5 * this->transAccelMax * this->smoothingDuration + this->transVel_tc; + this->transPos_ts3 = - sign * (3.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transVel_tc * this->smoothingDuration + this->transPos_tc; + + // Determine the time at the end of the second bang segment t_b2 + this->t_b2 = this->t_s3 + this->coastOptionBangDuration; + + // Determine the hub-relative position and velocity at time t_b2 + this->transVel_tb2 = - sign * this->transAccelMax * this->coastOptionBangDuration + this->transVel_ts3; + this->transPos_tb2 = - sign * 0.5 * this->transAccelMax * this->coastOptionBangDuration + * this->coastOptionBangDuration + this->transVel_ts3 * this->coastOptionBangDuration + + this->transPos_ts3; + + // Determine the time when the translation is complete t_f + this->t_f = this->t_b2 + this->smoothingDuration; } -/*! This method computes the required parameters for the translation with a coast period. +/*! This intermediate method groups the calculation of the current translational states into a single method. @return void */ -void PrescribedLinearTranslation::computeCoastParameters() { - if (this->transPosInit != this->transPosRef) { - // Determine the time at the end of the first ramp segment - this->tr = this->tInit + this->coastOptionRampDuration; - - // Determine the position and velocity at the end of the ramp segment/start of the coast segment - if (this->transPosInit < this->transPosRef) { - this->transPos_tr = (0.5 * this->transAccelMax * this->coastOptionRampDuration * this->coastOptionRampDuration) - + this->transPosInit; - this->transVel_tr = this->transAccelMax * this->coastOptionRampDuration; +void PrescribedLinearTranslation::computeCurrentState(double t) { + if (this->coastOptionBangDuration > 0.0) { + if(this->smoothingDuration > 0.0) { + if (this->isInFirstSmoothedSegment(t)) { + this->computeFirstSmoothedSegment(t); + } else if (this->isInFirstBangSegment(t)) { + this->computeFirstBangSegment(t); + } else if (this->isInSecondSmoothedSegment(t)) { + this->computeSecondSmoothedSegment(t); + } else if (this->isInCoastSegment(t)) { + this->computeCoastSegment(t); + } else if (this->isInThirdSmoothedSegment(t)) { + this->computeThirdSmoothedSegment(t); + } else if (this->isInSecondBangSegment(t)) { + this->computeSecondBangSegment(t); + } else if (this->isInFourthSmoothedSegment(t)) { + this->computeFourthSmoothedSegment(t); + } else { + this->computeTranslationComplete(); + } } else { - this->transPos_tr = - -((0.5 * this->transAccelMax * this->coastOptionRampDuration * this->coastOptionRampDuration)) - + this->transPosInit; - this->transVel_tr = -this->transAccelMax * this->coastOptionRampDuration; + if (this->isInFirstBangSegment(t)) { + this->computeFirstBangSegment(t); + } else if (this->isInCoastSegment(t)) { + this->computeCoastSegment(t); + } else if (this->isInSecondBangSegment(t)) { + this->computeSecondBangSegment(t); + } else { + this->computeTranslationComplete(); + } } + } else { + if (this->smoothingDuration > 0.0) { + if (this->isInFirstSmoothedSegment(t)) { + this->computeFirstSmoothedSegment(t); + } else if (this->isInFirstBangSegment(t)) { + this->computeFirstBangSegment(t); + } else if (this->isInSecondSmoothedSegment(t)) { + this->computeSecondSmoothedSegment(t); + } else if (this->isInSecondBangSegment(t)) { + this->computeSecondBangSegment(t); + } else if (this->isInThirdSmoothedSegment(t)) { + this->computeThirdSmoothedSegment(t); + } else { + this->computeTranslationComplete(); + } + } else { + if (this->isInFirstBangSegment(t)) { + this->computeFirstBangSegment(t); + } else if (this->isInSecondBangSegment(t)) { + this->computeSecondBangSegment(t); + } else { + this->computeTranslationComplete(); + } + } + } +} - // Determine the distance traveled during the coast period - double deltaPosCoast = this->transPosRef - this->transPosInit - 2 * (this->transPos_tr - this->transPosInit); - - // Determine the time duration of the coast segment - double tCoast = fabs(deltaPosCoast) / fabs(this->transVel_tr); +/*! This method determines if the current time is within the first bang segment. + @return bool + @param t [s] Current simulation time +*/ +bool PrescribedLinearTranslation::isInFirstBangSegment(double t) const { + if (this->smoothingDuration > 0.0) { + return (t > this->t_s1 && t <= this->t_b1 && this->t_f - this->tInit != 0.0); + } else { + return (t <= this->t_b1 && this->t_f - this->tInit != 0.0); + } +} - // Determine the time at the end of the coast segment - this->tc = this->tr + tCoast; +/*! This method determines if the current time is within the second bang segment. + @return bool + @param t [s] Current simulation time +*/ +bool PrescribedLinearTranslation::isInSecondBangSegment(double t) const { + if (this->coastOptionBangDuration > 0.0) { + if (this->smoothingDuration > 0.0) { + return (t > this->t_s3 && t <= this->t_b2 && this->t_f - this->tInit != 0.0); + } else { + return (t > this->t_c && t <= this->t_f && this->t_f - this->tInit != 0.0); + } + } else { + if (this->smoothingDuration > 0.0) { + return (t > this->t_s2 && t <= this->t_b2 && this->t_f - this->tInit != 0.0); + } else { + return (t > this->t_b1 && t <= this->t_f && this->t_f - this->tInit != 0.0); + } + } +} - // Determine the position [m] at the end of the coast segment - double transPos_tc = this->transPos_tr + deltaPosCoast; +/*! This method determines if the current time is within the first smoothing segment for the smoothed profiler options. + @return bool + @param t [s] Current simulation time +*/ +bool PrescribedLinearTranslation::isInFirstSmoothedSegment(double t) const { + return (t <= this->t_s1 && this->t_f - this->tInit != 0.0); +} - // Determine the time at the end of the translation - this->tf = this->tc + this->coastOptionRampDuration; +/*! This method determines if the current time is within the second smoothing segment for the smoothed profiler options.. + @return bool + @param t [s] Current simulation time +*/ +bool PrescribedLinearTranslation::isInSecondSmoothedSegment(double t) const { + return (t > this->t_b1 && t <= this->t_s2 && this->t_f - this->tInit != 0.0); +} - // Define the parabolic constants for the first and second ramp segments of the translation - this->a = (this->transPos_tr - this->transPosInit) / ((this->tr - this->tInit) * (this->tr - this->tInit)); - this->b = -(this->transPosRef - transPos_tc) / ((this->tc - this->tf) * (this->tc - this->tf)); +/*! This method determines if the current time is within the third smoothing segment for the smoothed profiler options. + @return bool + @param t [s] Current simulation time +*/ +bool PrescribedLinearTranslation::isInThirdSmoothedSegment(double t) const { + if (this->coastOptionBangDuration > 0.0) { + return (t > this->t_c && t <= this->t_s3 && this->t_f - this->tInit != 0.0); } else { - // If the initial position equals the reference position, no translation is required. - this->tf = this->tInit; + return (t > this->t_b2 && t <= this->t_f && this->t_f - this->tInit != 0.0); } } -/*! This method computes the scalar translational states for the coast option coast period. - @return void +/*! This method determines if the current time is within the fourth smoothing segment for the smoothed bang-coast-bang option. + @return bool @param t [s] Current simulation time */ -void PrescribedLinearTranslation::computeCoastSegment(double t) { - this->transAccel = 0.0; - this->transVel = this->transVel_tr; - this->transPos = this->transVel_tr * (t - this->tr) + this->transPos_tr; +bool PrescribedLinearTranslation::isInFourthSmoothedSegment(double t) const { + return (t > this->t_b2 && t <= this->t_f && this->t_f - this->tInit != 0.0); } -/*! This method determines if the current time is within the first ramp segment for the no coast option. +/*! This method determines if the current time is within the coast segment. @return bool @param t [s] Current simulation time */ -bool PrescribedLinearTranslation::isInFirstRampSegmentNoCoast(double t) const { - return (t <= this->ts && this->tf - this->tInit != 0); +bool PrescribedLinearTranslation::isInCoastSegment(double t) const { + if (this->smoothingDuration > 0.0) { + return (t > this->t_s2 && t <= this->t_c && this->t_f - this->tInit != 0.0); + } else{ + return (t > this->t_b1 && t <= this->t_c && this->t_f - this->tInit != 0.0); + } } -/*! This method determines if the current time is within the second ramp segment for the no coast option. - @return bool +/*! This method computes the first bang segment scalar translational states. + @return void @param t [s] Current simulation time */ -bool PrescribedLinearTranslation::isInSecondRampSegmentNoCoast(double t) const { - return (t > this->ts && t <= this->tf && this->tf - this->tInit != 0); +void PrescribedLinearTranslation::computeFirstBangSegment(double t) { + double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); + this->transAccel = sign * this->transAccelMax; + + if (this->smoothingDuration > 0.0) { + this->transVel = this->transAccel * (t - this->t_s1) + this->transVel_ts1; + this->transPos = 0.5 * this->transAccel * (t - this->t_s1) * (t - this->t_s1) + + this->transVel_ts1 * (t - this->t_s1) + this->transPos_ts1; + } else { + this->transVel = this->transAccel * (t - this->tInit); + this->transPos = this->a * (t - this->tInit) * (t - this->tInit) + this->transPosInit; + } } -/*! This method computes the required parameters for the translation with no coast period. +/*! This method computes the second bang segment scalar translational states. @return void + @param t [s] Current simulation time */ -void PrescribedLinearTranslation::computeParametersNoCoast() { - // Determine the total time required for the translation - double totalTransTime = sqrt(((0.5 * fabs(this->transPosRef - this->transPosInit)) * 8) / this->transAccelMax); +void PrescribedLinearTranslation::computeSecondBangSegment(double t) { + double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); + this->transAccel = - sign * this->transAccelMax; + + if (this->smoothingDuration > 0.0) { + if (this->coastOptionBangDuration > 0.0) { + this->transVel = this->transAccel * (t - this->t_s3) + this->transVel_ts3; + this->transPos = 0.5 * this->transAccel * (t - this->t_s3) * (t - this->t_s3) + + this->transVel_ts3 * (t - this->t_s3) + this->transPos_ts3; + } else { + this->transVel = this->transAccel * (t - this->t_s2) + this->transVel_ts2; + this->transPos = 0.5 * this->transAccel * (t - this->t_s2) * (t - this->t_s2) + + this->transVel_ts2 * (t - this->t_s2) + this->transPos_ts2; + } + } else { + this->transVel = this->transAccel * (t - this->t_f); + this->transPos = this->b * (t - this->t_f) * (t - this->t_f) + this->transPosRef; + } +} - // Determine the time at the end of the translation - this->tf = this->tInit + totalTransTime; +/*! This method computes the first smoothing segment scalar translational states for the smoothed profiler options. + @return void + @param t [s] Current simulation time +*/ +void PrescribedLinearTranslation::computeFirstSmoothedSegment(double t) { + double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); + + double term1 = (3.0 * (t - this->tInit) * (t - this->tInit)) / (this->smoothingDuration * this->smoothingDuration); + double term2 = (2.0 * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) + / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term3 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit)) + / (this->smoothingDuration * this->smoothingDuration); + double term4 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) + / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term5 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) + / (4.0 * this->smoothingDuration * this->smoothingDuration); + double term6 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) + / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + + this->transAccel = sign * this->transAccelMax * (term1 - term2); + this->transVel = sign * this->transAccelMax * (term3 - term4); + this->transPos = sign * this->transAccelMax * (term5 - term6) + this->transPosInit; +} - // Determine the time halfway through the translation - this->ts = this->tInit + (totalTransTime / 2); +/*! This method computes the second smoothing segment scalar translational states for the smoothed profiler options. + @return void + @param t [s] Current simulation time +*/ +void PrescribedLinearTranslation::computeSecondSmoothedSegment(double t) { + double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); + + double term1; + double term2; + double term3; + double term4; + double term5; + double term6; + double term7; + + if (this->coastOptionBangDuration > 0.0) { + term1 = (3.0 * (t - this->t_b1) * (t - this->t_b1)) / (this->smoothingDuration * this->smoothingDuration); + term2 = (2.0 * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) + / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) + / (this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) + / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term5 = 0.5 * (t - this->t_b1) * (t - this->t_b1); + term6 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) + / (4.0 * this->smoothingDuration * this->smoothingDuration); + term7 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) + / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + } else { + term1 = (3.0 * (t - this->t_b1) * (t - this->t_b1)) / (2.0 * this->smoothingDuration * this->smoothingDuration); + term2 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) + / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) + / (2.0 * this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) + / (8.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term5 = 0.5 * (t - this->t_b1) * (t - this->t_b1); + term6 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) + / (8.0 * this->smoothingDuration * this->smoothingDuration); + term7 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) + / (40.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + } - // Define the parabolic constants for the first and second half of the translation - this->a = 0.5 * (this->transPosRef - this->transPosInit) / ((this->ts - this->tInit) * (this->ts - this->tInit)); - this->b = -0.5 * (this->transPosRef - this->transPosInit) / ((this->ts - this->tf) * (this->ts - this->tf)); + this->transAccel = sign * this->transAccelMax * (1.0 - term1 + term2); + this->transVel = sign * this->transAccelMax * ((t - this->t_b1) - term3 + term4) + this->transVel_tb1; + this->transPos = sign * this->transAccelMax * (term5 - term6 + term7) + + this->transVel_tb1 * (t - this->t_b1) + this->transPos_tb1; } -/*! This method computes the scalar translational states for the first ramp segment. The acceleration during the first - * ramp segment is positive if the reference position is greater than the initial position. The acceleration is - * negative during the first ramp segment if the reference position is less than the initial position. +/*! This method computes the third smoothing segment scalar translational states for the smoothed profiler options. @return void @param t [s] Current simulation time */ -void PrescribedLinearTranslation::computeFirstRampSegment(double t) { - if (this->transPosInit < this->transPosRef) { - this->transAccel = this->transAccelMax; +void PrescribedLinearTranslation::computeThirdSmoothedSegment(double t) { + double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); + + double term1; + double term2; + double term3; + double term4; + double term5; + double term6; + double term7; + + if (this->coastOptionBangDuration > 0.0) { + term1 = (3.0 * (t - this->t_c) * (t - this->t_c)) / (this->smoothingDuration * this->smoothingDuration); + term2 = (2.0 * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) + / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c)) + / (this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) + / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term5 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) + / (4.0 * this->smoothingDuration * this->smoothingDuration); + term6 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) + / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + + this->transAccel = - sign * this->transAccelMax * (term1 - term2); + this->transVel = - sign * this->transAccelMax * (term3 - term4) + this->transVel_tc; + this->transPos = - sign * this->transAccelMax * (term5 - term6) + this->transVel_tc * (t - this->t_c) + this->transPos_tc; } else { - this->transAccel = -this->transAccelMax; + term1 = (3.0 * (t - this->t_b2) * (t - this->t_b2)) / (this->smoothingDuration * this->smoothingDuration); + term2 = (2.0 * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) + / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) + / (this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) + / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term5 = - 0.5 * (t - this->t_b2) * (t - this->t_b2); + term6 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) + / (4.0 * this->smoothingDuration * this->smoothingDuration); + term7 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) + / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + + this->transAccel = sign * this->transAccelMax * ( - 1.0 + term1 - term2); + this->transVel = sign * this->transAccelMax * ( - (t - this->t_b2) + term3 - term4) + this->transVel_tb2; + this->transPos = sign * this->transAccelMax * (term5 + term6 - term7) + this->transVel_tb2 * (t - this->t_b2) + + this->transPos_tb2; } - this->transVel = this->transAccel * (t - this->tInit); - this->transPos = this->a * (t - this->tInit) * (t - this->tInit) + this->transPosInit; } -/*! This method computes the scalar translational states for the second ramp segment. The acceleration during the - * second ramp segment is negative if the reference position is greater than the initial position. The acceleration - * is positive during the second ramp segment if the reference position is less than the initial position. +/*! This method computes the fourth smoothing segment scalar translational states for the smoothed bang-coast-bang option. + @return void + @param t [s] Current simulation time +*/ +void PrescribedLinearTranslation::computeFourthSmoothedSegment(double t) { + double term1 = (3.0 * (this->t_f - t) * (this->t_f - t)) / (this->smoothingDuration * this->smoothingDuration); + double term2 = (2.0 * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) + / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term3 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t)) + / (this->smoothingDuration * this->smoothingDuration); + double term4 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) + / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term5 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) + / (4.0 * this->smoothingDuration * this->smoothingDuration); + double term6 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) + / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + + double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); + + this->transAccel = - sign * this->transAccelMax * (term1 - term2); + this->transVel = sign * this->transAccelMax * (term3 - term4); + this->transPos = - sign * this->transAccelMax * (term5 - term6) + this->transPosRef; +} + +/*! This method computes the coast segment scalar translational states @return void @param t [s] Current simulation time */ -void PrescribedLinearTranslation::computeSecondRampSegment(double t) { - if (this->transPosInit < this->transPosRef) { - this->transAccel = -this->transAccelMax; +void PrescribedLinearTranslation::computeCoastSegment(double t) { + this->transAccel = 0.0; + + if (this->smoothingDuration > 0.0) { + this->transVel = this->transVel_ts2; + this->transPos = this->transVel_ts2 * (t - this->t_s2) + this->transPos_ts2; } else { - this->transAccel = this->transAccelMax; + this->transVel = this->transVel_tb1; + this->transPos = this->transVel_tb1 * (t - this->t_b1) + this->transPos_tb1; } - this->transVel = this->transAccel * (t - this->tInit) - this->transAccel * (this->tf - this->tInit); - this->transPos = this->b * (t - this->tf) * (t - this->tf) + this->transPosRef; } /*! This method computes the scalar translational states when the translation is complete. @@ -286,17 +633,55 @@ void PrescribedLinearTranslation::computeTranslationComplete() { this->convergence = true; } -/*! Setter method for the coast option ramp duration. +/*! This method writes the module output messages and computes the output message data. @return void - @param rampDuration [s] Ramp segment time duration */ -void PrescribedLinearTranslation::setCoastOptionRampDuration(double rampDuration) { - this->coastOptionRampDuration = rampDuration; +void PrescribedLinearTranslation::writeOutputMessages(uint64_t callTime) { + // Create the output buffer message + PrescribedTranslationMsgPayload prescribedTranslationMsgOut; + + // Zero the output messages + prescribedTranslationMsgOut = PrescribedTranslationMsgPayload(); + + // Compute the translational body position relative to the mount frame M expressed in M frame components + Eigen::Vector3d r_FM_M = this->transPos * this->transHat_M; // [m] + + // Compute the translational body velocity relative to the mount frame M expressed in M frame components + Eigen::Vector3d rPrime_FM_M = this->transVel * this->transHat_M; // [m/s] + + // Compute the translational body acceleration relative to the mount frame M expressed in M frame components + Eigen::Vector3d rPrimePrime_FM_M = this->transAccel * this->transHat_M; // [m/s^2] + + // Copy the module variables to the output buffer message + eigenVector3d2CArray(r_FM_M, prescribedTranslationMsgOut.r_FM_M); + eigenVector3d2CArray(rPrime_FM_M, prescribedTranslationMsgOut.rPrime_FM_M); + eigenVector3d2CArray(rPrimePrime_FM_M, prescribedTranslationMsgOut.rPrimePrime_FM_M); + + // Write the output messages + this->prescribedTranslationOutMsg.write(&prescribedTranslationMsgOut, this->moduleID, callTime); + PrescribedTranslationMsg_C_write(&prescribedTranslationMsgOut, + &prescribedTranslationOutMsgC, this->moduleID, callTime); } -/*! Setter method for the ramp segment scalar linear acceleration. +/*! Setter method for the coast option bang duration. @return void - @param transAccelMax [m/s^2] Ramp segment linear angular acceleration + @param coastOptionBangDuration [s] Bang segment time duration +*/ +void PrescribedLinearTranslation::setCoastOptionBangDuration(double coastOptionBangDuration) { + this->coastOptionBangDuration = coastOptionBangDuration; +} + +/*! Setter method for the duration the acceleration is smoothed until reaching the given maximum acceleration value. + @return void + @param smoothingDuration [s] Duration the acceleration is smoothed until reaching the given maximum acceleration value +*/ +void PrescribedLinearTranslation::setSmoothingDuration(double smoothingDuration) { + this->smoothingDuration = smoothingDuration; +} + +/*! Setter method for the bang segment scalar linear acceleration. + @return void + @param transAccelMax [m/s^2] Bang segment linear angular acceleration */ void PrescribedLinearTranslation::setTransAccelMax(double transAccelMax) { this->transAccelMax = transAccelMax; @@ -318,14 +703,21 @@ void PrescribedLinearTranslation::setTransPosInit(double transPosInit) { this->transPosInit = transPosInit; } -/*! Getter method for the coast option ramp duration. +/*! Getter method for the coast option bang duration. + @return double +*/ +double PrescribedLinearTranslation::getCoastOptionBangDuration() const { + return this->coastOptionBangDuration; +} + +/*! Getter method for the duration the acceleration is smoothed until reaching the given maximum acceleration value. @return double */ -double PrescribedLinearTranslation::getCoastOptionRampDuration() const { - return this->coastOptionRampDuration; +double PrescribedLinearTranslation::getSmoothingDuration() const { + return this->smoothingDuration; } -/*! Getter method for the ramp segment scalar linear acceleration. +/*! Getter method for the bang segment scalar linear acceleration. @return double */ double PrescribedLinearTranslation::getTransAccelMax() const { diff --git a/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.h b/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.h index fbecf950fa..6747818bb5 100755 --- a/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.h +++ b/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.h @@ -30,72 +30,95 @@ /*! @brief Prescribed Linear Translation Profiler Class */ class PrescribedLinearTranslation: public SysModel { public: + PrescribedLinearTranslation() = default; //!< Constructor + ~PrescribedLinearTranslation() = default; //!< Destructor + + void SelfInit() override; //!< Member function to initialize the C-wrapped output message + void Reset(uint64_t CurrentSimNanos) override; //!< Reset member function + void UpdateState(uint64_t CurrentSimNanos) override; //!< Update member function + void setCoastOptionBangDuration(double bangDuration); //!< Setter method for the coast option bang duration + void setSmoothingDuration(double smoothingDuration); //!< Setter method for the duration the acceleration is smoothed until reaching the given maximum acceleration value + void setTransAccelMax(double transAccelMax); //!< Setter method for the bang segment scalar linear acceleration + void setTransHat_M(const Eigen::Vector3d &transHat_M); //!< Setter method for the translating body axis of translation + void setTransPosInit(double transPosInit); //!< Setter method for the initial translating body hub-relative position + double getCoastOptionBangDuration() const; //!< Getter method for the coast option bang duration + double getSmoothingDuration() const; //!< Getter method for the duration the acceleration is smoothed until reaching the given maximum acceleration value + double getTransAccelMax() const; //!< Getter method for the bang segment scalar linear acceleration + const Eigen::Vector3d &getTransHat_M() const; //!< Getter method for the translating body axis of translation + double getTransPosInit() const; //!< Getter method for the initial translating body position - PrescribedLinearTranslation() = default; //!< Constructor - ~PrescribedLinearTranslation() = default; //!< Destructor - - void SelfInit() override; //!< Member function to initialize the C-wrapped output message - void Reset(uint64_t CurrentSimNanos) override; //!< Reset member function - void UpdateState(uint64_t CurrentSimNanos) override; //!< Update member function - void setCoastOptionRampDuration(double rampDuration); //!< Setter method for the coast option ramp duration - void setTransAccelMax(double transAccelMax); //!< Setter method for the ramp segment scalar linear acceleration - void setTransHat_M(const Eigen::Vector3d &transHat_M); //!< Setter method for the translating body axis of translation - void setTransPosInit(double transPosInit); //!< Setter method for the initial translating body hub-relative position - double getCoastOptionRampDuration() const; //!< Getter method for the coast option ramp duration - double getTransAccelMax() const; //!< Getter method for the ramp segment scalar linear acceleration - const Eigen::Vector3d &getTransHat_M() const; //!< Getter method for the translating body axis of translation - double getTransPosInit() const; //!< Getter method for the initial translating body position - ReadFunctor linearTranslationRigidBodyInMsg; //!< Input msg for the translational reference position and velocity Message prescribedTranslationOutMsg; //!< Output msg for the translational body prescribed states PrescribedTranslationMsg_C prescribedTranslationOutMsgC = {}; //!< C-wrapped Output msg for the translational body prescribed states - BSKLogger *bskLogger; //!< BSK Logging + BSKLogger *bskLogger; //!< BSK Logging private: - - /* Coast option member functions */ - bool isInFirstRampSegment(double time) const; //!< Method for determining if the current time is within the first ramp segment for the coast option - bool isInCoastSegment(double time) const; //!< Method for determining if the current time is within the coast segment for the coast option - bool isInSecondRampSegment(double time) const; //!< Method for determining if the current time is within the second ramp segment for the coast option - void computeCoastParameters(); //!< Method for computing the required parameters for the translation with a coast period - void computeCoastSegment(double time); //!< Method for computing the scalar translational states for the coast option coast period - - /* Non-coast option member functions */ - bool isInFirstRampSegmentNoCoast(double time) const; //!< Method for determining if the current time is within the first ramp segment for the no coast option - bool isInSecondRampSegmentNoCoast(double time) const; //!< Method for determining if the current time is within the second ramp segment for the no coast option - void computeParametersNoCoast(); //!< Method for computing the required parameters for the translation with no coast period - - /* Shared member functions */ - void computeFirstRampSegment(double time); //!< Method for computing the scalar translational states for the first ramp segment - void computeSecondRampSegment(double time); //!< Method for computing the scalar translational states for the second ramp segment - void computeTranslationComplete(); //!< Method for computing the scalar translational states when the translation is complete + /* Methods for computing the required translation parameters */ + void computeTranslationParameters(); //!< Intermediate method to group the calculation of translation parameters into a single method + void computeBangBangParametersNoSmoothing(); //!< Method for computing the required parameters for the non-smoothed bang-bang profiler option + void computeBangCoastBangParametersNoSmoothing(); //!< Method for computing the required parameters for the non-smoothed bang-coast-bang profiler option + void computeSmoothedBangBangParameters(); //!< Method for computing the required parameters for the translation with a smoothed bang-bang acceleration profile + void computeSmoothedBangCoastBangParameters(); //!< Method for computing the required parameters for the smoothed bang-coast-bang option + + /* Methods for computing the current translational states */ + void computeCurrentState(double time); //!< Intermediate method used to group the calculation of the current translational states into a single method + bool isInFirstBangSegment(double time) const; //!< Method for determining if the current time is within the first bang segment + bool isInSecondBangSegment(double time) const; //!< Method for determining if the current time is within the second bang segment + bool isInFirstSmoothedSegment(double time) const; //!< Method for determining if the current time is within the first smoothing segment for the smoothed profiler options + bool isInSecondSmoothedSegment(double time) const; //!< Method for determining if the current time is within the second smoothing segment for the smoothed profiler options + bool isInThirdSmoothedSegment(double time) const; //!< Method for determining if the current time is within the third smoothing segment for the smoothed profiler options + bool isInFourthSmoothedSegment(double time) const; //!< Method for determining if the current time is within the fourth smoothing segment for the smoothed bang-coast-bang option + bool isInCoastSegment(double time) const; //!< Method for determining if the current time is within the coast segment + void computeFirstBangSegment(double time); //!< Method for computing the first bang segment scalar translational states + void computeSecondBangSegment(double time); //!< Method for computing the second bang segment scalar translational states + void computeFirstSmoothedSegment(double time); //!< Method for computing the first smoothing segment scalar translational states for the smoothed profiler options + void computeSecondSmoothedSegment(double time); //!< Method for computing the second smoothing segment scalar translational states for the smoothed profiler options + void computeThirdSmoothedSegment(double time); //!< Method for computing the third smoothing segment scalar translational states for the smoothed profiler options + void computeFourthSmoothedSegment(double time); //!< Method for computing the fourth smoothing segment scalar translational states for the smoothed bang-coast-bang option + void computeCoastSegment(double time); //!< Method for computing the coast segment scalar translational states + void computeTranslationComplete(); //!< Method for computing the scalar translational states when the translation is complete + + void writeOutputMessages(uint64_t CurrentSimNanos); //!< Method for writing the module output messages and computing the output message data /* User-configurable variables */ - double coastOptionRampDuration; //!< [s] Ramp time used for the coast option - double transAccelMax; //!< [m/s^2] Maximum acceleration magnitude - Eigen::Vector3d transHat_M; //!< Axis along the direction of translation expressed in M frame components - - /* Coast option variables */ - double transPos_tr; //!< [m] Position at the end of the first ramp segment - double transVel_tr; //!< [m/s] Velocity at the end of the first ramp segment - double tr; //!< [s] The simulation time at the end of the first ramp segment - double tc; //!< [s] The simulation time at the end of the coast period - - /* Non-coast option variables */ - double ts; //!< [s] The simulation time halfway through the translation - - /* Shared module variables */ - double transPos; //!< [m] Current translational body position along transHat_M - double transVel; //!< [m] Current translational body velocity along transHat_M - double transAccel; //!< [m] Current translational body acceleration along transHat_M - bool convergence; //!< Boolean variable is true when the translation is complete - double tInit; //!< [s] Simulation time at the beginning of the translation - double transPosInit; //!< [m] Initial translational body position from M to F frame origin along transHat_M - double transPosRef; //!< [m] Reference translational body position from M to F frame origin along transHat_M - double tf; //!< [s] The simulation time when the translation is complete - double a; //!< Parabolic constant for the first half of the translation - double b; //!< Parabolic constant for the second half of the translation + double coastOptionBangDuration; //!< [s] Time used for the coast option bang segment + double smoothingDuration; //!< [s] Time the acceleration is smoothed to the given maximum acceleration value + double transAccelMax; //!< [m/s^2] Maximum acceleration magnitude + Eigen::Vector3d transHat_M; //!< Axis along the direction of translation expressed in M frame components + + /* Scalar translational states */ + double transPosInit; //!< [m] Initial translational body position from M to F frame origin along transHat_M + double transPosRef; //!< [m] Reference translational body position from M to F frame origin along transHat_M + double transPos; //!< [m] Current translational body position along transHat_M + double transVel; //!< [m] Current translational body velocity along transHat_M + double transAccel; //!< [m] Current translational body acceleration along transHat_M + double transPos_tb1; //!< [m] Position at the end of the first bang segment + double transVel_tb1; //!< [m/s] Velocity at the end of the first bang segment + double transPos_tb2; //!< [m] Position at the end of the second bang segment + double transVel_tb2; //!< [m/s] Velocity at the end of the second bang segment + double transPos_ts1; //!< [m] Position at the end of the first smoothed segment + double transVel_ts1; //!< [m/s] Velocity at the end of the first smoothed segment + double transPos_ts2; //!< [m] Position at the end of the second smoothed segment + double transVel_ts2; //!< [m/s] Velocity at the end of the second smoothed segment + double transPos_ts3; //!< [m] Position at the end of the third smoothed segment + double transVel_ts3; //!< [m/s] Velocity at the end of the third smoothed segment + double transPos_tc; //!< [m] Position at the end of the coast segment + double transVel_tc; //!< [m/s] Velocity at the end of the coast segment + + /* Temporal parameters */ + double tInit; //!< [s] Simulation time at the beginning of the translation + double t_b1; //!< [s] Simulation time at the end of the first bang segment + double t_b2; //!< [s] Simulation time at the end of the second bang segment + double t_s1; //!< [s] Simulation time at the end of the first smoothed segment + double t_s2; //!< [s] Simulation time at the end of the second smoothed segment + double t_s3; //!< [s] Simulation time at the end of the third smoothed segment + double t_c; //!< [s] Simulation time at the end of the coast segment + double t_f; //!< [s] Simulation time when the translation is complete + + bool convergence; //!< Boolean variable is true when the translation is complete + double a; //!< Parabolic constant for the first half of the bang-bang non-smoothed translation + double b; //!< Parabolic constant for the second half of the bang-bang non-smoothed translation }; #endif diff --git a/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.rst b/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.rst index 19aa890e2c..c201cff66c 100644 --- a/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.rst +++ b/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.rst @@ -4,21 +4,29 @@ This module profiles linear translational motion for a rigid body connected to a the translating body is designated by the frame :math:`\mathcal{F}`. The states of the translating body are profiled relative to a hub-fixed frame :math:`\mathcal{M}`. The :ref:`PrescribedTranslationMsgPayload` message is used to output the prescribed translational states from the module. The prescribed states profiled in this module are: ``r_FM_M``, -``rPrime_FM_M``, and ``rPrimePrime_FM_M``. This module has two options to profile the linear translation. +``rPrime_FM_M``, and ``rPrimePrime_FM_M``. This module has four options to profile the linear translation. The first option is a bang-bang acceleration profile that minimizes the time required to complete the translation. -The second option is a bang-off-bang acceleration profile that adds a coast period of zero acceleration between the -acceleration ramp segments. The module defaults to the bang-bang option with no coast period. If the coast option is -desired, the user must set the ramp time module variable ``coastOptionRampDuration`` to a nonzero value. +The second option is a bang-coast-bang acceleration profile that adds a coast period of zero acceleration between the +acceleration ramp segments. The third option is a smoothed bang-bang acceleration profile that uses cubic splines to +construct a continuous acceleration profile across the entire translation. The fourth option is a smoothed +bang-coast-bang acceleration profile. + +The module defaults to the non-smoothed bang-bang option with no coast period. If the coast option is desired, the +user must set the module variable ``coastOptionBangDuration`` to a nonzero value. If smoothing is desired, +the module variable ``smoothingDuration`` must be set to a nonzero value. .. important:: Note that this module assumes the initial and final hub-relative translational rates of the translating body are zero. The general inputs to this module that must be set by the user are the translational axis expressed as a -unit vector in Mount frame components ``transHat_M``, the initial translational body position relative to the Mount -frame ``transPosInit``, the reference position relative to the Mount frame ``transPosRef``, the maximum scalar linear -acceleration for the translation ``transAccelMax``, and the ramp time variable ``coastOptionRampDuration`` for specifying -the time the acceleration ramp segments are applied if the coast option is desired. This variable is defaulted to zero, -meaning that the module defaults to the bang-bang acceleration profile with no coast period. +unit vector in mount frame components ``transHat_M``, the initial translational body position relative to the hub-fixed +mount frame ``transPosInit``, the reference position relative to the mount frame ``transPosRef``, and the maximum scalar +linear acceleration for the translation ``transAccelMax``. The optional inputs ``coastOptionBangDuration`` and +``smoothingDuration`` can be set by the user to select the specific type of profiler that is desired. If these variables +are not set by the user, the module defaults to the non-smoothed bang-bang profiler. If only the variable +``coastOptionBangDuration`` is set to a nonzero value, the bang-coast-bang profiler is selected. If only the variable +``smoothingDuration`` is set to a nonzero value, the smoothed bang-bang profiler is selected. If both variables are +set to nonzero values, the smoothed bang-coast-bang profiler is selected. .. important:: To use this module for prescribed motion, it must be connected to the :ref:`PrescribedMotionStateEffector` @@ -53,8 +61,8 @@ provides information on what this message is used for. Detailed Module Description --------------------------- -Profiler With No Coast Period -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Non-Smoothed Bang-Bang Profiler +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ The first option to profile the linear translation is a pure bang-bang acceleration profile. If the given reference position is greater than the given initial position, the user-specified maximum acceleration value @@ -80,7 +88,7 @@ To profile this motion, the translational body's hub-relative scalar states :mat where .. math:: - a = \frac{ \rho_{\text{ref}} - \rho_0}{2 (t_s - t_0)^2} + a = \frac{ \rho_{\text{ref}} - \rho_0}{2 (t_{b1} - t_0)^2} During the second half of the translation the states are: @@ -96,25 +104,25 @@ During the second half of the translation the states are: where .. math:: - b = - \frac{ \rho_{\text{ref}} - \rho_0}{2 (t_s - t_f)^2} + b = - \frac{ \rho_{\text{ref}} - \rho_0}{2 (t_{b1} - t_f)^2} -The switch time :math:`t_s` is the simulation time halfway through the translation: +The switch time :math:`t_{b1}` is the simulation time at the end of the first bang segment: .. math:: - t_s = t_0 + \frac{\Delta t_{\text{tot}}}{2} + t_{b1} = t_0 + \frac{\Delta t_{\text{tot}}}{2} The total time required to complete the translation :math:`\Delta t_{\text{tot}}` is: .. math:: \Delta t_{\text{tot}} = 2 \sqrt{ \frac{| \rho_{\text{ref}} - \rho_0 | }{\ddot{\rho}_{\text{max}}}} = t_f - t_0 -Profiler With Coast Period -^^^^^^^^^^^^^^^^^^^^^^^^^^ +Non-Smoothed Bang-Coast-Bang Profiler +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ The second option to profile the linear translation is a bang-coast-bang acceleration profile with an added coast period between the acceleration segments where the acceleration is zero. Similar to the previous profiler, if the reference position is greater than the given initial position, the maximum acceleration value is applied -positively for the specified ramp time ``coastOptionRampDuration`` to the first segment of the translation and negatively +positively for the specified ramp time ``coastOptionBangDuration`` to the first segment of the translation and negatively to the third segment of the translation. The second segment of the translation is the coast period. However, if the reference position is less than the initial position, the acceleration is instead applied negatively during the first segment of the translation and positively during the third segment of the translation. As a result of this acceleration @@ -138,16 +146,17 @@ To profile this linear motion, the scalar translating body's hub-relative states where .. math:: - a = \frac{ \rho(t_r) - \rho_0}{2 (t_r - t_0)^2} + a = \frac{ \rho(t_{b1}) - \rho_0}{2 (t_{b1} - t_0)^2} -and :math:`\rho(t_r)` is the hub-relative position at the end of the first segment: +and :math:`\rho(t_{b1})` is the hub-relative position at the end of the first bang segment: .. math:: - \rho(t_r) = \pm \frac{1}{2} \ddot{\rho}_{\text{max}} t_{\text{ramp}}^2 + \dot{\rho}_0 t_{\text{ramp}} + \rho_0 + \rho(t_{b1}) = \pm \frac{1}{2} \ddot{\rho}_{\text{max}} t_{\text{bang}}^2 + \dot{\rho}_0 t_{\text{bang}} + \rho_0 .. important:: - Note the distinction between :math:`t_r` and :math:`t_{\text{ramp}}`. :math:`t_{\text{ramp}}` is the time duration of the acceleration segment - and :math:`t_r` is the simulation time at the end of the first acceleration segment. :math:`t_r = t_0 + t_{\text{ramp}}` + Note the distinction between :math:`t_{b1}` and :math:`t_{\text{bang}}`. :math:`t_{\text{bang}}` is the time + duration of the acceleration segment and :math:`t_{b1}` is the simulation time at the end of the first acceleration + segment. :math:`t_{b1} = t_0 + t_{\text{bang}}` During the coast segment, the translational states are: @@ -155,10 +164,10 @@ During the coast segment, the translational states are: \ddot{\rho}(t) = 0 .. math:: - \dot{\rho}(t) = \dot{\rho}(t_r) = \ddot{\rho}_{\text{max}} t_{\text{ramp}} + \dot{\rho}_0 + \dot{\rho}(t) = \dot{\rho}(t_{b1}) = \ddot{\rho}_{\text{max}} t_{\text{bang}} + \dot{\rho}_0 .. math:: - \rho(t) = \dot{\rho}(t_r) (t - t_r) + \rho(t_r) + \rho(t) = \dot{\rho}(t_{b1}) (t - t_{b1}) + \rho(t_{b1}) During the third segment, the translational states are @@ -179,60 +188,274 @@ where Here :math:`\rho(t_c)` is the hub-relative position at the end of the coast segment: .. math:: - \rho(t_c) = \rho(t_r) + \Delta \rho_{\text{coast}} + \rho(t_c) = \rho(t_{b1}) + \Delta \rho_{\text{coast}} and :math:`\Delta \rho_{\text{coast}}` is the distance traveled during the coast segment: .. math:: - \Delta \rho_{\text{coast}} = (\rho_{\text{ref}} - \rho_0) - 2 (\rho(t_r) - \rho_0) + \Delta \rho_{\text{coast}} = (\rho_{\text{ref}} - \rho_0) - 2 (\rho(t_{b1}) - \rho_0) :math:`t_c` is the simulation time at the end of the coast segment: .. math:: - t_c = t_r + \frac{\Delta \rho_{\text{coast}}}{\dot{\rho}(t_r)} + t_c = t_{b1} + \frac{\Delta \rho_{\text{coast}}}{\dot{\rho}(t_{b1})} Using the given translation axis ``transHat_M``, the scalar states are then transformed to the prescribed translational states ``r_FM_M``, ``rPrime_FM_M``, and ``rPrimePrime_FM_M``. The states are then written to the :ref:`PrescribedTranslationMsgPayload` module output message. +Smoothed Bang-Bang Profiler +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The third option to profile the linear translation is a smoothed bang-bang acceleration profile. This option is selected +by setting the module variable ``smoothingDuration`` to a nonzero value. This profiler uses cubic splines to construct +a continuous acceleration profiler across the entire translation. Similar to the non-smoothed bang-bang profiler, +this option smooths the acceleration between the given maximum acceleration values. +To profile this motion, the translational body's hub-relative scalar states :math:`\rho`, :math:`\dot{\rho}`, and +:math:`\ddot{\rho}` are prescribed as a function of time and the translational motion is split into five different +segments. + +The first segment smooths the acceleration from zero to the user-specified maximum acceleration value in the given +time ``smoothingDuration``. If the given reference position is greater than the given initial position, the +acceleration is smoothed positively to the given maximum acceleration value. If the given reference position is less +than the given initial position, the acceleration is smoothed from zero to the negative maximum acceleration value. +During this phase, the scalar hub-relative states are: + +.. math:: + \ddot{\rho}(t) = \pm \ddot{\rho}_{\text{max}} \left( \frac{3 (t - t_0)^2}{t_{\text{smooth}}^2} - \frac{2 (t - t_0)^3}{t_{\text{smooth}}^3} \right) + +.. math:: + \dot{\rho}(t) = \pm \ddot{\rho}_{\text{max}} \left( \frac{(t - t_0)^3}{t_{\text{smooth}}^2} - \frac{(t - t_0)^4}{2 t_{\text{smooth}}^3} \right) + +.. math:: + \rho(t) = \pm \ddot{\rho}_{\text{max}} \left( \frac{(t - t_0)^4}{4 t_{\text{smooth}}^2} - \frac{(t - t_0)^5}{10 t_{\text{smooth}}^3} \right) + +The second segment is the first bang segment where the maximum acceleration value is applied either positively or +negatively as discussed previously. The scalar hub-relative states during this phase are: + +.. math:: + \ddot{\rho}(t) = \pm \ddot{\rho}_{\text{max}} + +.. math:: + \dot{\rho}(t) = \pm \ddot{\rho}_{\text{max}} (t - t_{s1}) + \dot{\rho}(t_{s1}) + +.. math:: + \rho(t) = \pm \frac{\ddot{\rho}_{\text{max}} (t - t_{s1})^2}{2} + \dot{\rho}(t_{s1})(t - t_{s1}) + \rho(t_{s1}) + +where :math:`t_{s1}` is the time at the end of the first smoothing segment: + +.. math:: + t_{s1} = t_0 + t_{\text{smooth}} + +The third segment smooths the acceleration from the current maximum acceleration value to the opposite magnitude maximum +acceleration value. The scalar hub-relative states during this phase are: + +.. math:: + \ddot{\rho}(t) = \pm \ddot{\rho}_{\text{max}} \left( 1 - \frac{3 (t - t_{b1})^2}{2 t_{\text{smooth}}^2} + \frac{(t - t_{b1})^3}{2 t_{\text{smooth}}^3} \right) + +.. math:: + \dot{\rho}(t) = \pm \ddot{\rho}_{\text{max}} \left( (t - t_{b1}) - \frac{(t - t_{b1})^3}{2 t_{\text{smooth}}^2} + \frac{(t - t_{b1})^4}{8 t_{\text{smooth}}^3} \right) + \dot{\rho}(t_{b1}) + +.. math:: + \rho(t) = \pm \ddot{\rho}_{\text{max}} \left( \frac{(t - t_{b1})^2}{2} - \frac{(t - t_{b1})^4}{8 t_{\text{smooth}}^2} + \frac{(t - t_{b1})^5}{40 t_{\text{smooth}}^3} \right) + \dot{\rho}(t_{b1})(t - t_{b1}) + \rho(t_{b1}) + +where :math:`t_{b1}` is the time at the end of the first bang segment: + +.. math:: + t_{b1} = t_{s1} + t_{\text{bang}} + +The fourth segment is the second bang segment where the maximum acceleration value is applied either positively or +negatively as discussed previously. The scalar hub-relative states during this phase are: + +.. math:: + \ddot{\rho}(t) = \mp \ddot{\rho}_{\text{max}} + +.. math:: + \dot{\rho}(t) = \mp \ddot{\rho}_{\text{max}} (t - t_{s2}) + \dot{\rho}(t_{s2}) + +.. math:: + \rho(t) = \mp \frac{\ddot{\rho}_{\text{max}} (t - t_{s2})^2}{2} + \dot{\rho}(t_{s2})(t - t_{s2}) + \rho(t_{s2}) + +where :math:`t_{s2}` is the time at the end of the second smoothing segment: + +.. math:: + t_{s2} = t_{b1} + t_{\text{smooth}} + +The fifth segment is the third and final smoothing segment where the acceleration returns to zero. The scalar +hub-relative states during this phase are: + +.. math:: + \ddot{\rho}(t) = \mp \ddot{\rho}_{\text{max}} \left ( -1 + \frac{3(t - t_{b2})^2}{t_{\text{smooth}}^2} - \frac{2 (t - t_{b2})^3}{t_{\text{smooth}}^3} \right ) + +.. math:: + \dot{\rho}(t) = \mp \ddot{\rho}_{\text{max}} \left ( -(t - t_{b2}) + \frac{(t - t_{b2})^3}{t_{\text{smooth}}^2} - \frac{(t - t_{b2})^4}{2 t_{\text{smooth}}^3} \right ) + \dot{\rho}(t_{b2}) + +.. math:: + \rho(t) = \mp \ddot{\rho}_{\text{max}} \left ( \frac{(t - t_{b2})^2}{2} + \frac{(t - t_{b2})^4}{4 t_{\text{smooth}}^2} - \frac{(t - t_{b2})^5}{10 t_{\text{smooth}}^3} \right ) + \dot{\rho}(t_{b2})(t - t_{b2}) + \rho(t_{b2}) + +where :math:`t_{b2}` is the time at the end of the second bang segment: + +.. math:: + t_{b2} = t_{s2} + t_{\text{bang}} + +Smoothed Bang-Coast-Bang Profiler +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The fourth option to profile the linear translation is a smoothed bang-coast-bang acceleration profile. This option is +selected by setting the module variables ``coastOptionBangDuration`` and ``smoothingDuration`` to nonzero values. +This profiler uses cubic splines to construct a continuous acceleration profiler across the entire translation. +To profile this motion, the translational body's hub-relative scalar states :math:`\rho`, :math:`\dot{\rho}`, and +:math:`\ddot{\rho}` are prescribed as a function of time and the translational motion is split into seven different +segments. + +The first segment smooths the acceleration from zero to the user-specified maximum acceleration value in the given +time ``smoothingDuration``. If the given reference position is greater than the given initial position, the +acceleration is smoothed positively to the given maximum acceleration value. If the given reference position is less +than the given initial position, the acceleration is smoothed from zero to the negative maximum acceleration value. +During this phase, the scalar hub-relative states are: + +.. math:: + \ddot{\rho}(t) = \pm \ddot{\rho}_{\text{max}} \left( \frac{3 (t - t_0)^2}{t_{\text{smooth}}^2} - \frac{2 (t - t_0)^3}{t_{\text{smooth}}^3} \right) + +.. math:: + \dot{\rho}(t) = \pm \ddot{\rho}_{\text{max}} \left( \frac{(t - t_0)^3}{t_{\text{smooth}}^2} - \frac{(t - t_0)^4}{2 t_{\text{smooth}}^3} \right) + +.. math:: + \rho(t) = \pm \ddot{\rho}_{\text{max}} \left( \frac{(t - t_0)^4}{4 t_{\text{smooth}}^2} - \frac{(t - t_0)^5}{10 t_{\text{smooth}}^3} \right) + +The second segment is the first bang segment where the maximum acceleration value is applied either positively or +negatively as discussed previously. The scalar hub-relative states during this phase are: + +.. math:: + \ddot{\rho}(t) = \pm \ddot{\rho}_{\text{max}} + +.. math:: + \dot{\rho}(t) = \pm \ddot{\rho}_{\text{max}} (t - t_{s1}) + \dot{\rho}(t_{s1}) + +.. math:: + \rho(t) = \pm \frac{\ddot{\rho}_{\text{max}} (t - t_{s1})^2}{2} + \dot{\rho}(t_{s1})(t - t_{s1}) + \rho(t_{s1}) + +where :math:`t_{s1}` is the time at the end of the first smoothing segment. + +The third segment prior to the coast phase smooths the acceleration from the current maximum acceleration value to zero. +The scalar hub-relative states during this phase are: + +.. math:: + \ddot{\rho}(t) = \pm \ddot{\rho}_{\text{max}} \left( 1 - \frac{3 (t - t_{b1})^2}{t_{\text{smooth}}^2} - \frac{2 (t - t_{b1})^3}{t_{\text{smooth}}^3} \right) + +.. math:: + \dot{\rho}(t) = \pm \ddot{\rho}_{\text{max}} \left( (t - t_{b1}) - \frac{(t - t_{b1})^3}{t_{\text{smooth}}^2} - \frac{(t - t_{b1})^4}{2 t_{\text{smooth}}^3} \right) + \dot{\rho}(t_{b1}) + +.. math:: + \rho(t) = \pm \ddot{\rho}_{\text{max}} \left( \frac{(t - t_{b1})^2}{2} - \frac{(t - t_{b1})^4}{4 t_{\text{smooth}}^2} - \frac{(t - t_{b1})^5}{10 t_{\text{smooth}}^3} \right) + \dot{\rho}(t_{b1})(t - t_{b1}) + \rho(t_{b1}) + +where :math:`t_{b1}` is the time at the end of the first bang segment. + +The fourth segment is the coast segment where the translational states are: + +.. math:: + \ddot{\rho}(t) = 0 + +.. math:: + \dot{\rho}(t) = \dot{\rho}(t_{s2}) + +.. math:: + \rho(t) = \dot{\rho}(t_{s2}) (t - t_{s2}) + \rho(t_{s2}) + +where :math:`t_{s2}` is the time at the end of the second smoothing segment. + +The fifth segment smooths the acceleration from zero to the maximum acceleration value prior to the second bang segment. +The translational states during this phase are: + +.. math:: + \ddot{\rho}(t) = \mp \ddot{\rho}_{\text{max}} \left( \frac{3 (t - t_c)^2}{t_{\text{smooth}}^2} - \frac{2 (t - t_c)^3}{t_{\text{smooth}}^3} \right) + +.. math:: + \dot{\rho}(t) = \mp \ddot{\rho}_{\text{max}} \left( \frac{(t - t_c)^3}{t_{\text{smooth}}^2} - \frac{(t - t_c)^4}{2 t_{\text{smooth}}^3} \right) + \dot{\rho}(t_c) + +.. math:: + \rho(t) = \mp \ddot{\rho}_{\text{max}} \left( \frac{(t - t_c)^4}{4 t_{\text{smooth}}^2} - \frac{(t - t_c)^5}{10 t_{\text{smooth}}^3} \right) + \dot{\rho}(t_c) (t - t_c) + \rho(t_c) + +where :math:`t_c` is the time at the end of the coast segment. + +The sixth segment is the second bang segment where the maximum acceleration value is applied either positively or +negatively as discussed previously. The scalar hub-relative states during this phase are: + +.. math:: + \ddot{\rho}(t) = \mp \ddot{\rho}_{\text{max}} + +.. math:: + \dot{\rho}(t) = \mp \ddot{\rho}_{\text{max}} (t - t_{s3}) + \dot{\rho}(t_{s3}) + +.. math:: + \rho(t) = \mp \frac{\ddot{\rho}_{\text{max}} (t - t_{s3})^2}{2} + \dot{\rho}(t_{s3})(t - t_{s3}) + \rho(t_{s3}) + +where :math:`t_{s3}` is the time at the end of the third smoothing segment. + +The seventh segment is the fourth and final smoothing segment where the acceleration returns to zero. The scalar +hub-relative states during this phase are: + +.. math:: + \ddot{\rho}(t) = \mp \ddot{\rho}_{\text{max}} \left (\frac{3(t_f - t)^2}{t_{\text{smooth}}^2} - \frac{2 (t_f - t)^3}{t_{\text{smooth}}^3} \right ) + +.. math:: + \dot{\rho}(t) = \pm \ddot{\rho}_{\text{max}} \left (\frac{(t_f - t)^3}{t_{\text{smooth}}^2} - \frac{(t_f - t)^4}{2 t_{\text{smooth}}^3} \right ) + +.. math:: + \rho(t) = \mp \ddot{\rho}_{\text{max}} \left (\frac{(t_f - t)^4}{4 t_{\text{smooth}}^2} - \frac{(t_f - t)^5}{10 t_{\text{smooth}}^3} \right ) + \rho_{\text{ref}} + +where :math:`t_f` is the time at the end of the translation: + Module Testing ^^^^^^^^^^^^^^ -The unit test for this module ensures that a profiled linear translation for a secondary rigid body connected to a -spacecraft hub is properly computed for several different simulation configurations. The unit test profiles two -successive translations to ensure the module is correctly configured. The 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. The unit test also tests both methods of profiling the translation, where either a pure bang-bang -acceleration profile can be selected for the translation, or a coast option can be selected where the accelerations -are only applied for a specified ramp time and a coast segment with zero acceleration is applied between the two -acceleration periods. To validate the module, the final hub-relative position at the end of each translation is -checked to match the specified reference position. +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. The 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. + +The 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. + +To verify the module functionality, the final position at the end of each translation segment is checked to match +the specified reference positions. Additionally, for the smoothed profiler options, the numerical derivative of the +profiled displacements and velocities is determined across the entire simulation. 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. User Guide ---------- The general inputs to this module that must be set by the user are the translational axis expressed as a -unit vector in Mount frame components ``transHat_M``, the initial translational body position relative to the Mount -frame ``transPosInit``, the reference position relative to the Mount frame ``transPosRef``, the maximum scalar linear -acceleration for the translation ``transAccelMax``, and the ramp time variable ``coastOptionRampDuration`` for specifying -the time the acceleration ramp segments are applied if the coast option is desired. This variable is defaulted to zero, -meaning that the module defaults to the bang-bang acceleration profile with no coast period. +unit vector in mount frame components ``transHat_M``, the initial translational body position relative to the hub-fixed +mount frame ``transPosInit``, the reference position relative to the mount frame ``transPosRef``, and the maximum scalar +linear acceleration for the translation ``transAccelMax``. The optional inputs ``coastOptionBangDuration`` and +``smoothingDuration`` can be set by the user to select the specific type of profiler that is desired. If these variables +are not set by the user, the module defaults to the non-smoothed bang-bang profiler. If only the variable +``coastOptionBangDuration`` is set to a nonzero value, the bang-coast-bang profiler is selected. If only the variable +``smoothingDuration`` is set to a nonzero value, the smoothed bang-bang profiler is selected. If both variables are +set to nonzero values, the smoothed bang-coast-bang profiler is selected. This section is to outline the steps needed to setup the prescribed linear translational module in python using Basilisk. -#. Import the prescribedTranslation class:: +#. Import the prescribedLinearTranslation class:: - from Basilisk.simulation import prescribedTranslation + from Basilisk.simulation import prescribedLinearTranslation #. Create an instantiation of the module:: prescribedLinearTrans = prescribedLinearTranslation.PrescribedLinearTranslation() -#. Define all of the configuration data associated with the module. For example, to configure the coast option:: +#. Define all of the configuration data associated with the module. For example, to configure the smoothed bang-coast-bang option:: prescribedLinearTrans.ModelTag = "prescribedLinearTranslation" prescribedLinearTrans.setTransHat_M(np.array([0.5, 0.0, 0.5 * np.sqrt(3)])) prescribedLinearTrans.setTransAccelMax(0.01) # [m/s^2] prescribedLinearTrans.setTransPosInit(0.5) # [m] prescribedLinearTrans.setCoastRampDuration(1.0) # [s] + prescribedLinearTrans.setSmoothingDuration(1.0) # [s] #. Connect a :ref:`LinearTranslationRigidBodyMsgPayload` message for the translating body reference position to the module. For example, the user can create a stand-alone message to specify the reference position::