diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/_UnitTest/test_prescribedRot1DOF.py b/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/_UnitTest/test_prescribedRot1DOF.py deleted file mode 100755 index 7dc53e82a..000000000 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/_UnitTest/test_prescribedRot1DOF.py +++ /dev/null @@ -1,200 +0,0 @@ -# -# ISC License -# -# Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder -# -# Permission to use, copy, modify, and/or distribute this software for any -# purpose with or without fee is hereby granted, provided that the above -# copyright notice and this permission notice appear in all copies. -# -# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES -# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF -# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR -# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES -# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN -# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF -# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. -# - - -# -# Unit Test Script -# Module Name: prescribedRot1DOF -# Author: Leah Kiner -# Creation Date: Nov 14, 2022 -# - -import pytest -import inspect -import matplotlib.pyplot as plt -import numpy as np -import os -from Basilisk.architecture import bskLogging -from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import prescribedRot1DOF # import the module that is to be tested -from Basilisk.utilities import RigidBodyKinematics as rbk -from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport - -filename = inspect.getframeinfo(inspect.currentframe()).filename -path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' -splitPath = path.split(bskName) - - -# Vary the initial angle, reference angle, and maximum angular acceleration for pytest -@pytest.mark.parametrize("thetaInit", [0, 2*np.pi/3]) -@pytest.mark.parametrize("thetaRef", [0, 2*np.pi/3]) -@pytest.mark.parametrize("thetaDDotMax", [0.008, 0.1]) -@pytest.mark.parametrize("accuracy", [1e-12]) -def test_prescribedRot1DOFTestFunction(show_plots, thetaInit, thetaRef, thetaDDotMax, accuracy): - r""" - **Validation Test Description** - - This unit test ensures that the profiled 1 DOF rotation for a secondary rigid body connected - to the spacecraft hub is properly computed for a series of initial and reference PRV angles and maximum - angular accelerations. The final prescribed attitude and angular velocity magnitude are compared with - the reference values. - - **Test Parameters** - - Args: - thetaInit (float): [rad] Initial PRV angle of the F frame with respect to the M frame - thetaRef (float): [rad] Reference PRV angle of the F frame with respect to the M frame - thetaDDotMax (float): [rad/s^2] Maximum angular acceleration for the attitude maneuver - accuracy (float): absolute accuracy value used in the validation tests - - **Description of Variables Being Tested** - - This unit test ensures that the profiled 1 DOF rotation is properly computed for a series of initial and - reference PRV angles and maximum angular accelerations. The final prescribed angle ``theta_FM_Final`` - and angular velocity magnitude ``thetaDot_Final`` are compared with the reference values ``theta_Ref`` and - ``thetaDot_Ref``, respectively. - """ - [testResults, testMessage] = prescribedRot1DOFTestFunction(show_plots, thetaInit, thetaRef, thetaDDotMax, accuracy) - - assert testResults < 1, testMessage - - -def prescribedRot1DOFTestFunction(show_plots, thetaInit, thetaRef, thetaDDotMax, accuracy): - """Call this routine directly to run the unit test.""" - testFailCount = 0 - testMessages = [] - unitTaskName = "unitTask" - unitProcessName = "TestProcess" - bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) - - # Create a sim module as an empty container - unitTestSim = SimulationBaseClass.SimBaseClass() - - # Create the test thread - testProcessRate = macros.sec2nano(0.1) - testProc = unitTestSim.CreateNewProcess(unitProcessName) - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - - # Create an instance of the prescribedRot1DOF module to be tested - PrescribedRot1DOF = prescribedRot1DOF.PrescribedRot1DOF() - PrescribedRot1DOF.modelTag = "prescribedRot1DOF" - - # Add the prescribedRot1DOF test module to runtime call list - unitTestSim.AddModelToTask(unitTaskName, PrescribedRot1DOF) - - # Initialize the prescribedRot1DOF test module configuration data - rotAxisM = np.array([1.0, 0.0, 0.0]) - prvInit_FM = thetaInit * rotAxisM - PrescribedRot1DOF.rotAxis_M = rotAxisM - PrescribedRot1DOF.thetaDDotMax = thetaDDotMax - PrescribedRot1DOF.omega_FM_F = np.array([0.0, 0.0, 0.0]) - PrescribedRot1DOF.omegaPrime_FM_F = np.array([0.0, 0.0, 0.0]) - PrescribedRot1DOF.sigma_FM = rbk.PRV2MRP(prvInit_FM) - - # Create the prescribedRot1DOF input message - thetaDotRef = 0.0 # [rad/s] - HingedRigidBodyMessageData = messaging.HingedRigidBodyMsgPayload() - HingedRigidBodyMessageData.theta = thetaRef - HingedRigidBodyMessageData.thetaDot = thetaDotRef - HingedRigidBodyMessage = messaging.HingedRigidBodyMsg().write(HingedRigidBodyMessageData) - PrescribedRot1DOF.spinningBodyInMsg.subscribeTo(HingedRigidBodyMessage) - - # Log the test module output message for data comparison - dataLog = PrescribedRot1DOF.prescribedRotationOutMsg.recorder() - unitTestSim.AddModelToTask(unitTaskName, dataLog) - - # Initialize the simulation - unitTestSim.InitializeSimulation() - - # Set the simulation time - simTime = np.sqrt(((0.5 * np.abs(thetaRef - thetaInit)) * 8) / thetaDDotMax) + 1 - unitTestSim.ConfigureStopTime(macros.sec2nano(simTime)) - - # Begin the simulation - unitTestSim.ExecuteSimulation() - - # Extract the logged data for plotting and data comparison - omega_FM_F = dataLog.omega_FM_F - sigma_FM = dataLog.sigma_FM - timespan = dataLog.times() - - thetaDot_Final = np.linalg.norm(omega_FM_F[-1, :]) - sigma_FM_Final = sigma_FM[-1, :] - theta_FM_Final = 4 * np.arctan(np.linalg.norm(sigma_FM_Final)) - - # Convert the logged sigma_FM MRPs to a scalar theta_FM array - n = len(timespan) - theta_FM = [] - for i in range(n): - theta_FM.append(4 * np.arctan(np.linalg.norm(sigma_FM[i, :]))) - - # Plot theta_FM - thetaRef_plotting = np.ones(len(timespan)) * thetaRef - thetaInit_plotting = np.ones(len(timespan)) * thetaInit - plt.figure() - plt.clf() - plt.plot(timespan * macros.NANO2SEC, theta_FM, label=r"$\Phi$") - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * thetaRef_plotting, '--', label=r'$\Phi_{Ref}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * thetaInit_plotting, '--', label=r'$\Phi_{0}$') - plt.title(r'$\Phi_{\mathcal{F}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) - plt.ylabel('(deg)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='center right', prop={'size': 16}) - - # Plot omega_FM_F - plt.figure() - plt.clf() - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_FM_F[:, 0], label=r'$\omega_{1}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_FM_F[:, 1], label=r'$\omega_{2}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_FM_F[:, 2], label=r'$\omega_{3}$') - plt.title(r'${}^\mathcal{F} \omega_{\mathcal{F}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) - plt.ylabel('(deg/s)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='upper right', prop={'size': 16}) - - if show_plots: - plt.show() - plt.close("all") - - # Check to ensure the initial angle rate converged to the reference angle rate - if not unitTestSupport.isDoubleEqual(thetaDot_Final, thetaDotRef, accuracy): - testFailCount += 1 - testMessages.append("FAILED: " + PrescribedRot1DOF.modelTag + "thetaDot_Final and thetaDotRef do not match") - - # Check to ensure the initial angle converged to the reference angle - if not unitTestSupport.isDoubleEqual(theta_FM_Final, thetaRef, accuracy): - testFailCount += 1 - testMessages.append("FAILED: " + PrescribedRot1DOF.modelTag + "theta_FM_Final and thetaRef do not match") - return [testFailCount, ''.join(testMessages)] - - -# -# This statement below ensures that the unitTestScript can be run as a -# stand-along python script -# -if __name__ == "__main__": - prescribedRot1DOFTestFunction( - True, - np.pi/6, # thetaInit - 2*np.pi/3, # thetaRef - 0.008, # thetaDDotMax - 1e-12 # accuracy - ) diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.cpp b/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.cpp deleted file mode 100755 index 3c5c942ee..000000000 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.cpp +++ /dev/null @@ -1,158 +0,0 @@ -/* - ISC License - - Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder - - Permission to use, copy, modify, and/or distribute this software for any - purpose with or without fee is hereby granted, provided that the above - copyright notice and this permission notice appear in all copies. - - THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES - WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF - MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR - ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES - WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN - ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF - OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. - - */ - -/* Import the module header file */ -#include "prescribedRot1DOF.h" - -/* Other required files to import */ -#include -#include -#include "architecture/utilities/linearAlgebra.h" -#include "architecture/utilities/rigidBodyKinematics.h" -#include "architecture/utilities/macroDefinitions.h" - -/*! This method performs a complete reset of the module. The input messages are checked to ensure they are linked. - @return void - @param callTime [ns] Time the method is called -*/ -void PrescribedRot1DOF::reset(uint64_t callTime) -{ - // Check if the required input message is linked - if (!this->spinningBodyInMsg.isLinked()) - { - this->bskLogger.bskLog(BSK_ERROR, "Error: prescribedRot1DOF.spinningBodyInMsg wasn't connected."); - } - - // Set the initial time - this->tInit = 0.0; - - // Set the initial convergence to true to enter the required loop in Update_prescribedRot1DOF() 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. - @return void - @param callTime [ns] Time the method is called -*/ -void PrescribedRot1DOF::updateState(uint64_t callTime) -{ - // Create the buffer messages - HingedRigidBodyMsgPayload spinningBodyIn = {}; - HingedRigidBodyMsgPayload spinningBodyOut = {}; - PrescribedRotationMsgPayload prescribedRotationOut = {}; - - if (this->spinningBodyInMsg.isWritten()) - { - spinningBodyIn = this->spinningBodyInMsg(); - } - - /* This loop is entered (a) initially and (b) when each attitude maneuver is complete. The reference angle is updated - even if a new message is not written */ - if (this->spinningBodyInMsg.timeWritten() <= callTime && this->convergence) - { - // Store the initial time as the current simulation time - this->tInit = callTime * NANO2SEC; - - // Calculate the current ange and angle rate - double prv_FM_array[3]; - MRP2PRV(this->sigma_FM, prv_FM_array); - this->thetaInit = v3Dot(prv_FM_array, this->rotAxis_M); - this->thetaDotInit = v3Norm(this->omega_FM_F); - - // Store the reference angle and reference angle rate - this->thetaRef = spinningBodyIn.theta; - this->thetaDotRef = spinningBodyIn.thetaDot; - - // Define temporal information for the maneuver - double convTime = sqrt(((0.5 * fabs(this->thetaRef - this->thetaInit)) * 8) / this->thetaDDotMax); - this->tf = this->tInit + convTime; - this->ts = this->tInit + convTime / 2; - - // Define the parabolic constants for the first and second half of the maneuver - this->a = 0.5 * (this->thetaRef - this->thetaInit) / ((this->ts - this->tInit) * (this->ts - this->tInit)); - this->b = -0.5 * (this->thetaRef - this->thetaInit) / ((this->ts - this->tf) * (this->ts - this->tf)); - - // Set the convergence to false until the attitude maneuver is complete - this->convergence = false; - } - - // Store the current simulation time - double t = callTime * NANO2SEC; - - // Define the scalar prescribed states - double thetaDDot; - double thetaDot; - double theta; - - // Compute the prescribed scalar states at the current simulation time - if ((t < this->ts || t == this->ts) && this->tf - this->tInit != 0) // Entered during the first half of the maneuver - { - thetaDDot = this->thetaDDotMax; - thetaDot = thetaDDot * (t - this->tInit) + this->thetaDotInit; - theta = this->a * (t - this->tInit) * (t - this->tInit) + this->thetaInit; - } - else if ( t > this->ts && t <= this->tf && this->tf - this->tInit != 0) // Entered during the second half of the maneuver - { - thetaDDot = -1 * this->thetaDDotMax; - thetaDot = thetaDDot * (t - this->tInit) + this->thetaDotInit - thetaDDot * (this->tf - this->tInit); - theta = this->b * (t - this->tf) * (t - this->tf) + this->thetaRef; - } - else // Entered when the maneuver is complete - { - thetaDDot = 0.0; - thetaDot = this->thetaDotRef; - theta = this->thetaRef; - this->convergence = true; - } - - // Determine dcm_FF0 - double dcm_FF0[3][3]; - double prv_FF0_array[3]; - double theta_FF0 = theta - this->thetaInit; - v3Scale(theta_FF0, this->rotAxis_M, prv_FF0_array); - PRV2C(prv_FF0_array, dcm_FF0); - - // Determine dcm_F0M - double dcm_F0M[3][3]; - double prv_F0M_array[3]; - v3Scale(this->thetaInit, this->rotAxis_M, prv_F0M_array); - PRV2C(prv_F0M_array, dcm_F0M); - - // Determine dcm_FM - double dcm_FM[3][3]; - m33MultM33(dcm_FF0, dcm_F0M, dcm_FM); - - // Determine the prescribed parameter: sigma_FM - C2MRP(dcm_FM, this->sigma_FM); - - // Copy the module variables to the prescribedRotationOut output message - v3Copy(this->omega_FM_F, prescribedRotationOut.omega_FM_F); - v3Copy(this->omegaPrime_FM_F, prescribedRotationOut.omegaPrime_FM_F); - v3Copy(this->sigma_FM, prescribedRotationOut.sigma_FM); - - // Copy the local scalar variables to the spinningBodyOut output message - spinningBodyOut.theta = theta; - spinningBodyOut.thetaDot = thetaDot; - - // Write the output messages - this->spinningBodyOutMsg.write(&spinningBodyOut, this->moduleID, callTime); - this->prescribedRotationOutMsg.write(&prescribedRotationOut, this->moduleID, callTime); -} diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.h b/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.h deleted file mode 100755 index 12737e402..000000000 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.h +++ /dev/null @@ -1,64 +0,0 @@ -/* - ISC License - - Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder - - Permission to use, copy, modify, and/or distribute this software for any - purpose with or without fee is hereby granted, provided that the above - copyright notice and this permission notice appear in all copies. - - THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES - WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF - MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR - ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES - WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN - ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF - OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. - */ - -#ifndef _PRESCRIBEDROT1DOF_ -#define _PRESCRIBEDROT1DOF_ - -#include -#include -#include "architecture/utilities/bskLogging.h" -#include "architecture/_GeneralModuleFiles/sys_model.h" -#include "architecture/messaging/messaging.h" -#include "architecture/msgPayloadDefC/HingedRigidBodyMsgPayload.h" -#include "architecture/msgPayloadDefC/PrescribedRotationMsgPayload.h" - -/*! @brief Top level structure for the sub-module routines. */ -class PrescribedRot1DOF : public SysModel { -public: - void reset(uint64_t callTime) override; - void updateState(uint64_t callTime) override; - - /* User configurable variables */ - double thetaDDotMax; //!< [rad/s^2] Maximum angular acceleration of spinning body - double rotAxis_M[3]; //!< Rotation axis for the maneuver in M frame components - double omega_FM_F[3]; //!< [rad/s] Angular velocity of frame F wrt frame M in F frame components - double omegaPrime_FM_F[3]; //!< [rad/s^2] B frame time derivative of omega_FM_F in F frame components - double sigma_FM[3]; //!< MRP attitude of frame F with respect to frame M - - /* Private variables */ - bool convergence; //!< Boolean variable is true when the maneuver is complete - double tInit; //!< [s] Simulation time at the beginning of the maneuver - double thetaInit; //!< [rad] Initial spinning body angle from frame M to frame F about rotAxis_M - double thetaDotInit; //!< [rad/s] Initial spinning body angle rate between frame M to frame F - double thetaRef; //!< [rad] Reference angle from frame M to frame F about rotAxis_M - double thetaDotRef; //!< [rad/s] Reference angle rate between frame M to frame F - double ts; //!< [s] The simulation time halfway through the maneuver (switch time for ang accel) - double tf; //!< [s] Simulation time when the maneuver is finished - double a; //!< Parabolic constant for the first half of the maneuver - double b; //!< Parabolic constant for the second half of the maneuver - - BSKLogger bskLogger={}; //!< BSK Logging - - /* Messages */ - ReadFunctor spinningBodyInMsg; //!< Input msg for the spinning body reference angle and angle rate - Message spinningBodyOutMsg; //!< Output msg for the spinning body angle and angle rate - Message prescribedRotationOutMsg; //!< Output msg for the spinning body prescribed rotational states - -}; - -#endif diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.i b/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.i deleted file mode 100755 index cf542e706..000000000 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.i +++ /dev/null @@ -1,39 +0,0 @@ -/* - ISC License - - Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder - - Permission to use, copy, modify, and/or distribute this software for any - purpose with or without fee is hereby granted, provided that the above - copyright notice and this permission notice appear in all copies. - - THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES - WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF - MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR - ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES - WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN - ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF - OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. - - */ -%module prescribedRot1DOF -%{ - #include "prescribedRot1DOF.h" -%} - -%pythoncode %{ - from Basilisk.architecture.swig_common_model import * -%} - -%include "sys_model.i" -%include "swig_conly_data.i" - -%include "prescribedRot1DOF.h" - -%include "architecture/msgPayloadDefC/HingedRigidBodyMsgPayload.h" -%include "architecture/msgPayloadDefC/PrescribedRotationMsgPayload.h" - -%pythoncode %{ -import sys -protectAllClasses(sys.modules[__name__]) -%} diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.rst b/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.rst deleted file mode 100644 index c7d3a9afb..000000000 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.rst +++ /dev/null @@ -1,171 +0,0 @@ -Executive Summary ------------------ -This module profiles a :ref:`PrescribedRotationMsgPayload` message for a specified 1 DOF rotation for a secondary -prescribed rigid body connected to a rigid spacecraft hub at a hub-fixed location, :math:`\mathcal{M}`. The body -frame for the prescribed body is designated by the frame :math:`\mathcal{F}`. Accordingly, the prescribed states for the -secondary body are written with respect to the mount frame, :math:`\mathcal{M}`. The prescribed states profiled -in this module are: ``omega_FM_F``, ``omegaPrime_FM_F``, and ``sigma_FM``. - -To use this module for prescribed motion, it must be connected to the :ref:`PrescribedMotionStateEffector` -dynamics module in order to profile the rotational states of the secondary body. A second kinematic profiler -module must also be connected to the prescribed motion dynamics module to profile the translational states of the -prescribed body. The required rotation is determined from the user-specified scalar maximum angular acceleration -for the rotation :math:`\alpha_{\text{max}}`, prescribed body's initial attitude with respect to the mount frame -as the Principal Rotation Vector ``prv_F0M`` :math:`(\Phi_0, \hat{\textbf{{e}}}_0)`, and the prescribed body's -reference attitude with respect to the mount frame as the Principal Rotation Vector -``prv_F1M`` :math:`(\Phi_1, \hat{\textbf{{e}}}_1)`. - -The maximum scalar angular acceleration is applied constant and positively for the first half of the rotation and -constant negatively for the second half of the rotation. The resulting angular velocity of the prescribed body is -linear, approaching a maximum magnitude halfway through the rotation and ending with zero residual velocity. -The corresponding angle the prescribed body moves through during the rotation is parabolic in time. - -.. warning:: - This module is now deprecated. See the :ref:`PrescribedRotation1DOF` module that replaces this module. - -Message Connection Descriptions -------------------------------- -The following table lists all the module input and output messages. -The module msg connection is set by the user from python. -The msg type contains a link to the message structure definition, while the description -provides information on what this message is used for. - -.. list-table:: Module I/O Messages - :widths: 25 25 50 - :header-rows: 1 - - * - Msg Variable Name - - Msg Type - - Description - * - spinningBodyInMsg - - :ref:`HingedRigidBodyMsgPayload` - - input msg with the scalar spinning body rotational reference states - * - spinningBodyOutMsg - - :ref:`HingedRigidBodyMsgPayload` - - output message with the profiled scalar spinning body rotational states - * - prescribedMotionOutMsg - - :ref:`PrescribedRotationMsgPayload` - - output message with the profiled prescribed spinning body rotational states - - - -Detailed Module Description ---------------------------- -This 1 DOF rotational motion kinematic profiler module is written to profile spinning body motion with respect to a -body-fixed mount frame. The inputs to the profiler are the scalar maximum angular acceleration for the rotation -:math:`\alpha_{\text{max}}`, the prescribed body's initial attitude with respect to the mount frame as the Principal -Rotation Vector ``prv_F0M`` :math:`(\Phi_0, \hat{\textbf{{e}}}_0)`, and the prescribed body's reference attitude with -respect to the mount frame as the Principal Rotation Vector ``prv_F1M`` :math:`(\Phi_1, \hat{\textbf{{e}}}_1)`. -The prescribed body is assumed to be non-rotating at the beginning of the rotation. - -Subtracting the initial principal rotation vector from the reference principal rotation vector gives the required -rotation angle and axis for the rotation: - -.. math:: - \Phi_{\text{ref}} = 2 \cos^{-1} \left ( \cos \frac{\Phi_1}{2} \cos \frac{\Phi_0}{2} + \sin \frac{\Phi_1}{2} \sin \frac {\Phi_0}{2} \hat{\textbf{{e}}}_1 \cdot \hat{\textbf{{e}}}_0 \right ) - -.. math:: - \hat{\textbf{{e}}} = \frac{\cos \frac{\Phi_0}{2} \sin \frac{\Phi_1}{2} \hat{\textbf{{e}}}_1 - \cos \frac{\Phi_1}{2} \sin \frac{\Phi_0}{2} \hat{\textbf{{e}}}_0 + \sin \frac{\Phi_1}{2} \sin \frac{\Phi_0}{2} \hat{\textbf{{e}}}_1 \times \hat{\textbf{{e}}}_0 }{\sin \frac{\Phi_{\text{ref}}}{2}} - -During the first half of the rotation, the prescribed body is constantly accelerated with the given maximum -angular acceleration. The prescribed body's angular velocity increases linearly during the acceleration phase and -reaches a maximum magnitude halfway through the rotation. The switch time :math:`t_s` is the simulation time -halfway through the rotation: - -.. math:: - t_s = t_0 + \frac{\Delta t}{2} - -where the time required for the rotation :math:`\Delta t` is determined using the inputs to the profiler: - -.. math:: - \Delta t = t_f - t_0 = 2 \sqrt{ \Phi_{\text{ref}} / \ddot{\Phi}_{\text{max}}} - -The resulting trajectory of the angle :math:`\Phi` swept during the first half of the rotation is parabolic. The profiled -motion is concave upwards if the reference angle :math:`\Phi_{\text{ref}}` is greater than zero. If the converse is true, -the profiled motion is instead concave downwards. The described motion during the first half of the rotation -is characterized by the expressions: - -.. math:: - \omega_{\mathcal{F} / \mathcal{M}}(t) = \alpha_{\text{max}} - -.. math:: - \dot{\Phi}(t) = \alpha_{\text{max}} (t - t_0) - -.. math:: - \Phi(t) = c_1 (t - t_0)^2 - -where - -.. math:: - c_1 = \frac{\Phi_{\text{ref}}}{2(t_s - t_0)^2} - -Similarly, the second half of the rotation decelerates the prescribed body constantly until it reaches a -non-rotating state. The prescribed body angular velocity decreases linearly from its maximum magnitude back to zero. -The trajectory swept during the second half of the rotation is quadratic and concave downwards if the reference angle -:math:`\Phi_{\text{ref}}` is positive. If :math:`\Phi_{\text{ref}}` is negative, the profiled motion is instead -concave upwards. The described motion during the second half of the rotation is characterized by the expressions: - -.. math:: - \ddot{\Phi}(t) = -\alpha_{\text{max}} - -.. math:: - \dot{\Phi}(t) = \alpha_{\text{max}} (t - t_f) - -.. math:: - \Phi(t) = c_2 (t - t_f)^2 + \Phi_{\text{ref}} - - where - -.. math:: - c_2 = \frac{\Phi_{\text{ref}}}{2(t_s - t_f)^2} - -Module Testing -^^^^^^^^^^^^^^ -The unit test for this module ensures that the profiled 1 DOF rotation is properly computed for a series of -initial and reference PRV angles and maximum angular accelerations. The final prescribed angle ``theta_FM_Final`` -and angular velocity magnitude ``thetaDot_Final`` are compared with the reference values ``theta_Ref`` and -``thetaDot_Ref``, respectively. - -User Guide ----------- -The user-configurable inputs to the profiler are the scalar maximum angular acceleration for the rotation -:math:`\alpha_{\text{max}}`, the prescribed body's initial attitude with respect to the mount frame as the Principal -Rotation Vector ``prv_F0M`` :math:`(\Phi_0, \hat{\textbf{{e}}}_0)`, and the prescribed body's reference attitude with -respect to the mount frame as the Principal Rotation Vector ``prv_F1M`` :math:`(\Phi_1, \hat{\textbf{{e}}}_1)`. - -This module provides two output messages in the form of :ref:`HingedRigidBodyMsgPayload` and -:ref:`PrescribedRotationMsgPayload`. The first message describes the spinning body's scalar rotational states relative -to the body-fixed mount frame. The second prescribed rotational motion output message can be connected to the -:ref:`PrescribedMotionStateEffector` dynamics module to directly profile a state effector's rotational motion. Note -that a separate translational profiler module must also be connected to the prescribed motion dynamics module to fully -define the kinematic motion of the prescribed body. - -This section is to outline the steps needed to setup a prescribed 1 DOF rotational module in python using Basilisk. - -#. Import the prescribedRot1DOF class:: - - from Basilisk.fswAlgorithms import prescribedRot1DOF - -#. Create an instantiation of a prescribed rotational 1 DOF C module and the associated C++ container:: - - PrescribedRot1DOF = prescribedRot1DOF.prescribedRot1DOF() - PrescribedRot1DOF.modelTag = "prescribedRot1DOF" - -#. Define all of the configuration data associated with the module. For example:: - - thetaInit = 0.0 # [rad] - rotAxis_M = np.array([1.0, 0.0, 0.0]) - prvInit_FM = thetaInit * rotAxisM - PrescribedRot1DOF.rotAxis_M = rotAxis_M - PrescribedRot1DOF.thetaDDotMax = 0.01 # [rad/s^2] - PrescribedRot1DOF.omega_FM_F = np.array([0.0, 0.0, 0.0]) - PrescribedRot1DOF.omegaPrime_FM_F = np.array([0.0, 0.0, 0.0]) - PrescribedRot1DOF.sigma_FM = rbk.PRV2MRP(prvInit_FM) - -The user is required to set the above configuration data parameters, as they are not initialized in the module. - -#. Make sure to connect the required messages for this module. - -#. Add the module to the task list:: - - unitTestSim.AddModelToTask(unitTaskName, PrescribedRot1DOF)