From d1916c6183a39e0a7e8130407b2df84c65f77a89 Mon Sep 17 00:00:00 2001 From: Leah Kiner Date: Fri, 9 Feb 2024 15:29:17 -0700 Subject: [PATCH 1/7] Add a new prescribed translational state message This new message contains only the translational states of a prescribed body relative to a hub-fixed mount frame --- .../PrescribedTranslationMsgPayload.h | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 src/architecture/msgPayloadDefC/PrescribedTranslationMsgPayload.h diff --git a/src/architecture/msgPayloadDefC/PrescribedTranslationMsgPayload.h b/src/architecture/msgPayloadDefC/PrescribedTranslationMsgPayload.h new file mode 100644 index 0000000000..230b7ac77f --- /dev/null +++ b/src/architecture/msgPayloadDefC/PrescribedTranslationMsgPayload.h @@ -0,0 +1,32 @@ +/* + ISC License + + Copyright (c) 2024, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef prescribedTranslationSimMsg_h +#define prescribedTranslationSimMsg_h + + + /*! @brief Structure used to define the prescribed motion state effector translational state data message */ +typedef struct { + double r_FM_M[3]; //!< [m] Position vector from the M frame origin to the F frame origin expressed in M frame components + double rPrime_FM_M[3]; //!< [m/s] B/M frame time derivative of r_FM_M + double rPrimePrime_FM_M[3]; //!< [m/s^2] B/M frame time derivative of rPrime_FM_M +}PrescribedTranslationMsgPayload; + + +#endif /* prescribedTranslationSimMsg_h */ From 8f294a76ccf57ab1bc2930f058606659994eaf7f Mon Sep 17 00:00:00 2001 From: Leah Kiner Date: Fri, 9 Feb 2024 15:30:44 -0700 Subject: [PATCH 2/7] Add a new prescribed rotational state message This new message contains only the rotational states of a prescribed body relative to a hub-fixed mount frame --- .../PrescribedRotationMsgPayload.h | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 src/architecture/msgPayloadDefC/PrescribedRotationMsgPayload.h diff --git a/src/architecture/msgPayloadDefC/PrescribedRotationMsgPayload.h b/src/architecture/msgPayloadDefC/PrescribedRotationMsgPayload.h new file mode 100644 index 0000000000..bd1384bd1d --- /dev/null +++ b/src/architecture/msgPayloadDefC/PrescribedRotationMsgPayload.h @@ -0,0 +1,32 @@ +/* + ISC License + + Copyright (c) 2024, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef prescribedRotationSimMsg_h +#define prescribedRotationSimMsg_h + + + /*! @brief Structure used to define the prescribed motion state effector rotational state data message */ +typedef struct { + double omega_FM_F[3]; //!< [rad/s] Angular velocity of the F frame wrt the M frame in F frame components + double omegaPrime_FM_F[3]; //!< [rad/s^2] B/M frame time derivative of omega_FM_F + double sigma_FM[3]; //!< MRP attitude parameters for the F frame relative to the M frame +}PrescribedRotationMsgPayload; + + +#endif /* prescribedRotationSimMsg_h */ From 3c45d4569509581a324b3be9bc7cfe313d7008ee Mon Sep 17 00:00:00 2001 From: Leah Kiner Date: Fri, 9 Feb 2024 15:50:28 -0700 Subject: [PATCH 3/7] Refactor prescribedTrans module output message The module output message is changed from the prescribedMotionMsgPayload message containing both the translational and rotational states of a prescribed body to only the translational states via the new prescribedTranslationMsgPayload message --- .../_UnitTest/test_prescribedTrans.py | 26 ++--- .../prescribedTrans/prescribedTrans.c | 17 ++- .../prescribedTrans/prescribedTrans.h | 7 +- .../prescribedTrans/prescribedTrans.i | 4 +- .../prescribedTrans/prescribedTrans.rst | 100 +++++++++--------- 5 files changed, 71 insertions(+), 83 deletions(-) diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedTrans/_UnitTest/test_prescribedTrans.py b/src/fswAlgorithms/effectorInterfaces/prescribedTrans/_UnitTest/test_prescribedTrans.py index 72b6d26249..b7e2ab3066 100755 --- a/src/fswAlgorithms/effectorInterfaces/prescribedTrans/_UnitTest/test_prescribedTrans.py +++ b/src/fswAlgorithms/effectorInterfaces/prescribedTrans/_UnitTest/test_prescribedTrans.py @@ -30,12 +30,11 @@ import os import pytest from Basilisk.architecture import bskLogging -from Basilisk.architecture import messaging # import the message definitions +from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import prescribedTrans # 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 # general support file with common unit test functions +from Basilisk.utilities import unitTestSupport filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) @@ -53,7 +52,7 @@ def test_prescribedTransTestFunction(show_plots, scalarPosInit, scalarPosRef, sc r""" **Validation Test Description** - This unit test ensures that the profiled translational maneuver for a secondary rigid body connected + This unit test ensures that the profiled translation for a secondary prescribed rigid body connected to the spacecraft hub is properly computed for a series of initial and reference positions and maximum accelerations. The final prescribed position and velocity magnitudes are compared with the reference values. @@ -67,7 +66,7 @@ def test_prescribedTransTestFunction(show_plots, scalarPosInit, scalarPosRef, sc **Description of Variables Being Tested** - This unit test ensures that the profiled translational maneuver is properly computed for a series of initial and + This unit test ensures that the profiled translation is properly computed for a series of initial and reference positions and maximum accelerations. The final prescribed position magnitude ``r_FM_M_Final`` and velocity magnitude ``rPrime_FM_M_Final`` are compared with the reference values ``r_FM_M_Ref`` and ``rPrime_FM_M_Ref``, respectively. @@ -80,23 +79,23 @@ def test_prescribedTransTestFunction(show_plots, scalarPosInit, scalarPosRef, sc def prescribedTransTestFunction(show_plots, scalarPosInit, scalarPosRef, scalarAccelMax, accuracy): """Call this routine directly to run the unit test.""" - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 + testMessages = [] + unitTaskName = "unitTask" + unitProcessName = "TestProcess" bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(0.1) # update process rate update time + testProcessRate = macros.sec2nano(0.1) testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm and associated C++ container PrescribedTrans = prescribedTrans.prescribedTrans() - PrescribedTrans.ModelTag = "prescribedTrans" # update python name of test module + PrescribedTrans.ModelTag = "prescribedTrans" # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, PrescribedTrans) @@ -108,9 +107,6 @@ def prescribedTransTestFunction(show_plots, scalarPosInit, scalarPosRef, scalarA PrescribedTrans.r_FM_M = scalarPosInit * transAxis_M PrescribedTrans.rPrime_FM_M = np.array([0.0, 0.0, 0.0]) PrescribedTrans.rPrimePrime_FM_M = np.array([0.0, 0.0, 0.0]) - PrescribedTrans.omega_FM_F = np.array([0.0, 0.0, 0.0]) - PrescribedTrans.omegaPrime_FM_F = np.array([0.0, 0.0, 0.0]) - PrescribedTrans.sigma_FM = np.array([0.0, 0.0, 0.0]) # Create input message scalarVelRef = 0.0 # [m/s] @@ -121,7 +117,7 @@ def prescribedTransTestFunction(show_plots, scalarPosInit, scalarPosRef, scalarA PrescribedTrans.linearTranslationRigidBodyInMsg.subscribeTo(linearTranslationRigidBodyMessage) # Setup logging on the test module output message so that we get all the writes to it - dataLog = PrescribedTrans.prescribedMotionOutMsg.recorder() + dataLog = PrescribedTrans.prescribedTranslationOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) # Need to call the self-init and cross-init methods diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.c b/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.c index c461080b9b..950f97f2d9 100755 --- a/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.c +++ b/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.c @@ -35,7 +35,7 @@ void SelfInit_prescribedTrans(PrescribedTransConfig *configData, int64_t moduleID) { // Initialize the module output message - PrescribedMotionMsg_C_init(&configData->prescribedMotionOutMsg); + PrescribedTranslationMsg_C_init(&configData->prescribedTranslationOutMsg); } /*! This method performs a complete reset of the module. Local module variables that retain @@ -72,10 +72,10 @@ void Update_prescribedTrans(PrescribedTransConfig *configData, uint64_t callTime { // Create the buffer messages LinearTranslationRigidBodyMsgPayload linearTranslationRigidBodyIn; - PrescribedMotionMsgPayload prescribedMotionOut; + PrescribedTranslationMsgPayload prescribedTranslationOut; // Zero the output message - prescribedMotionOut = PrescribedMotionMsg_C_zeroMsgPayload(); + prescribedTranslationOut = PrescribedTranslationMsg_C_zeroMsgPayload(); // Read the input message linearTranslationRigidBodyIn = LinearTranslationRigidBodyMsg_C_zeroMsgPayload(); @@ -144,13 +144,10 @@ void Update_prescribedTrans(PrescribedTransConfig *configData, uint64_t callTime v3Scale(scalarAccel, configData->transAxis_M, configData->rPrimePrime_FM_M); // Copy the local variables to the output message - v3Copy(configData->r_FM_M, prescribedMotionOut.r_FM_M); - v3Copy(configData->rPrime_FM_M, prescribedMotionOut.rPrime_FM_M); - v3Copy(configData->rPrimePrime_FM_M, prescribedMotionOut.rPrimePrime_FM_M); - v3Copy(configData->omega_FM_F, prescribedMotionOut.omega_FM_F); - v3Copy(configData->omegaPrime_FM_F, prescribedMotionOut.omegaPrime_FM_F); - v3Copy(configData->sigma_FM, prescribedMotionOut.sigma_FM); + v3Copy(configData->r_FM_M, prescribedTranslationOut.r_FM_M); + v3Copy(configData->rPrime_FM_M, prescribedTranslationOut.rPrime_FM_M); + v3Copy(configData->rPrimePrime_FM_M, prescribedTranslationOut.rPrimePrime_FM_M); // Write the prescribed motion output message - PrescribedMotionMsg_C_write(&prescribedMotionOut, &configData->prescribedMotionOutMsg, moduleID, callTime); + PrescribedTranslationMsg_C_write(&prescribedTranslationOut, &configData->prescribedTranslationOutMsg, moduleID, callTime); } diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.h b/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.h index 4e551b49db..be070d921e 100755 --- a/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.h +++ b/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.h @@ -22,7 +22,7 @@ #include #include #include "architecture/utilities/bskLogging.h" -#include "cMsgCInterface/PrescribedMotionMsg_C.h" +#include "cMsgCInterface/PrescribedTranslationMsg_C.h" #include "cMsgCInterface/LinearTranslationRigidBodyMsg_C.h" /*! @brief Top level structure for the sub-module routines. */ @@ -34,9 +34,6 @@ typedef struct { double r_FM_M[3]; //!< [m] Position of the frame F origin with respect to the M frame origin expressed in M frame components double rPrime_FM_M[3]; //!< [m/s] B frame time derivative of r_FM_M expressed in M frame components double rPrimePrime_FM_M[3]; //!< [m/s^] B frame time derivative of rPrime_FM_M expressed in M frame components - double omega_FM_F[3]; //!< [rad/s] Angular velocity of frame F with respect to frame M expressed in F frame components - double omegaPrime_FM_F[3]; //!< [rad/s^2] B frame time derivative of omega_FM_F expressed 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 @@ -52,7 +49,7 @@ typedef struct { // Messages LinearTranslationRigidBodyMsg_C linearTranslationRigidBodyInMsg; //!< Input message for the reference states - PrescribedMotionMsg_C prescribedMotionOutMsg; //!< Output message for the prescribed states + PrescribedTranslationMsg_C prescribedTranslationOutMsg; //!< Output message for the prescribed translational states BSKLogger *bskLogger; //!< BSK Logging diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.i b/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.i index 43169e5fb1..40992b2564 100755 --- a/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.i +++ b/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.i @@ -24,8 +24,8 @@ %include "swig_c_wrap.i" %c_wrap_2(prescribedTrans, PrescribedTransConfig); -%include "architecture/msgPayloadDefC/PrescribedMotionMsgPayload.h" -struct PrescribedMotionMsg_C; +%include "architecture/msgPayloadDefC/PrescribedTranslationMsgPayload.h" +struct PrescribedTranslationMsg_C; %include "architecture/msgPayloadDefC/LinearTranslationRigidBodyMsgPayload.h" struct LinearTranslationRigidBodyMsg_C; diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.rst b/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.rst index 9e773736f3..75658cbc92 100644 --- a/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.rst +++ b/src/fswAlgorithms/effectorInterfaces/prescribedTrans/prescribedTrans.rst @@ -1,24 +1,23 @@ Executive Summary ----------------- -This module profiles a :ref:`PrescribedMotionMsgPayload` message for a specified translational maneuver -for a secondary 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 are: ``r_FM_M``, -``rPrime_FM_M``, ``rPrimePrime_FM_M``, ``omega_FM_F``, ``omegaPrime_FM_F``, and ``sigma_FM``. Because this is a -purely translational profiler, the states ``omega_FM_F``, ``omegaPrime_FM_F``, and ``sigma_FM`` are held -constant in this module. - -To use this module for prescribed motion purposes, it must be connected to the :ref:`PrescribedMotionStateEffector` -dynamics module in order to profile the states of the secondary body. The required maneuver is determined from the -user-specified scalar maximum acceleration :math:`a_{\text{max}}`, the mount frame axis for the translational motion, -the prescribed body's initial position vector with respect to the mount frame :math:`\boldsymbol{r}_{F/M}(t_0)`, and -the reference position vector or the prescribed body with respect to the mount frame -:math:`\boldsymbol{r}_{F/M} (\text{ref})`. - -The maximum scalar acceleration is applied constant and positively for the first half of the maneuver and -constant negatively for the second half of the maneuver. The resulting velocity of the prescribed body is -linear, approaching a maximum magnitude halfway through the maneuver and ending with zero residual velocity. -The corresponding translational trajectory the prescribed body moves through during the maneuver is parabolic in time. +This module profiles a :ref:`PrescribedTranslationMsgPayload` message for a specified 1 DOF translation +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: ``r_FM_M``, ``rPrime_FM_M``, and ``rPrimePrime_FM_M``. + +To use this module for prescribed motion, it must be connected to the :ref:`PrescribedMotionStateEffector` +dynamics module in order to profile the translational states of the secondary body. A second kinematic profiler +module must also be connected to the prescribed motion dynamics module to profile the rotational states of the +prescribed body. The required translation is determined from the user-specified scalar maximum acceleration +:math:`a_{\text{max}}`, the mount frame axis for the translational motion, the prescribed body's initial position +vector with respect to the mount frame :math:`\boldsymbol{r}_{F/M}(t_0)`, and the reference position vector or the +prescribed body with respect to the mount frame :math:`\boldsymbol{r}_{F/M} (\text{ref})`. + +The maximum scalar acceleration is applied constant and positively for the first half of the translation and +constant negatively for the second half of the translation. The resulting velocity of the prescribed body is +linear, approaching a maximum magnitude halfway through the translation and ending with zero residual velocity. +The corresponding translational trajectory the prescribed body moves through during the translation is parabolic in time. Message Connection Descriptions @@ -38,21 +37,20 @@ provides information on what this message is used for. * - linearTranslationRigidBodyInMsg - :ref:`LinearTranslationRigidBodyMsgPayload` - input msg with the prescribed body reference states - * - prescribedMotionOutMsg - - :ref:`PrescribedMotionMsgPayload` - - output message with the prescribed body states - + * - prescribedTranslationOutMsg + - :ref:`PrescribedTranslationMsgPayload` + - output message with the prescribed body translational states Detailed Module Description --------------------------- -This translational motion flight software module is written to profile a rigid body's motion with respect to a hub-fixed -mount frame. The inputs to the profiler are the scalar maximum acceleration for the maneuver :math:`a_{\text{max}}`, -the mount frame axis for the translational motion, the prescribed body's initial position vector with respect to -the mount frame :math:`\boldsymbol{r}_{F/M}(t_0)`, and the reference position vector or the prescribed body with respect -to the mount frame :math:`\boldsymbol{r}_{F/M} (\text{ref})`. The magnitudes of the initial and final position vectors -are denoted :math:`r_0` and :math:`r_{\text{ref}}`, respectively. The prescribed body is assumed to be at rest at the -beginning of the attitude maneuver. +This translational motion kinematic profiler module is written to profile a rigid body's translational motion with +respect to a hub-fixed mount frame. The inputs to the profiler are the scalar maximum acceleration for the translation +:math:`a_{\text{max}}`, the mount frame axis for the translational motion, the prescribed body's initial position +vector with respect to the mount frame :math:`\boldsymbol{r}_{F/M}(t_0)`, and the reference position vector or the +prescribed body with respect to the mount frame :math:`\boldsymbol{r}_{F/M} (\text{ref})`. +The magnitudes of the initial and final position vectors are denoted :math:`r_0` and :math:`r_{\text{ref}}`, +respectively. The prescribed body is assumed to be at rest at the beginning of the translation. Subtracting the initial position from the reference position vector gives the required relative position vector in the direction of translation: @@ -61,24 +59,25 @@ direction of translation: \Delta \boldsymbol{r} = \boldsymbol{r}_{F/M}(\text{ref}) - \boldsymbol{r}_{F/M}(t_0) The magnitude of the determined relative position vector gives the required translational distance :math:`\Delta r`. -During the first half of the maneuver, the prescribed body is constantly accelerated with the given maximum acceleration. -The prescribed body's velocity increases linearly during the acceleration phase and reaches a maximum magnitude halfway -through the maneuver. +During the first half of the translation, the prescribed body is constantly accelerated with the given maximum +acceleration. The prescribed body's velocity increases linearly during the acceleration phase and reaches a maximum +magnitude halfway through the translation. -The switch time, :math:`t_s` is the simulation time halfway through the maneuver: +The switch time, :math:`t_s` is the simulation time halfway through the translation: .. math:: t_s = t_0 + \frac{\Delta t}{2} -The time required for the maneuver :math:`\Delta t` is determined using the inputs to the profiler: +The time required for the translation :math:`\Delta t` is determined using the inputs to the profiler: .. math:: \Delta t = \sqrt{\frac{4 r_{\text{ref}} - 8 r_0}{\ddot{a}_{\text{max}}}} -The resulting trajectory of the position vector :math:`r = || \boldsymbol{r}_{F/M} ||_2` magnitude during the first half of the -maneuver is parabolic. The profiled motion is concave upwards if the reference position magnitude :math:`r_{\text{ref}}` -is greater than the initial position magnitude :math:`r_0`. If the converse is true, the profiled motion is instead concave -downwards. The described motion during the first half of the maneuver is characterized by the expressions: +The resulting trajectory of the position vector :math:`r = || \boldsymbol{r}_{F/M} ||_2` magnitude during the first +half of the translation is parabolic. The profiled motion is concave upwards if the reference position magnitude +:math:`r_{\text{ref}}` is greater than the initial position magnitude :math:`r_0`. If the converse is true, +the profiled motion is instead concave downwards. The described motion during the first half of the translation +is characterized by the expressions: .. math:: r^{''}_{F / M}(t) = a_{\text{max}} @@ -95,11 +94,11 @@ where c_1 = \frac{r_{\text{ref}} - r_0}{2(t_s - t_0)^2} -Similarly, the second half of the maneuver decelerates the prescribed body constantly until it reaches the desired +Similarly, the second half of the translation decelerates the prescribed body constantly until it reaches the desired position with zero velocity. The prescribed body velocity decreases linearly from its maximum magnitude back to zero. -The trajectory during the second half of the maneuver is quadratic and concave downwards if the reference position +The trajectory during the second half of the translation is quadratic and concave downwards if the reference position magnitude is greater than the initial position magnitude. If the converse is true, the profiled motion is instead -concave upwards. The described motion during the second half of the maneuver is characterized by the expressions: +concave upwards. The described motion during the second half of the translation is characterized by the expressions: .. math:: r^{''}_{F / M}(t) = -a_{\text{max}} @@ -117,20 +116,22 @@ where Module Testing ^^^^^^^^^^^^^^ -This unit test for this module ensures that the profiled translational maneuver is properly computed for a series of +This unit test for this module ensures that the profiled translation is properly computed for a series of initial and reference positions and maximum accelerations. The final prescribed position magnitude ``r_FM_M_Final`` and velocity magnitude ``rPrime_FM_M_Final`` are compared with the reference values ``r_FM_M_Ref`` and ``rPrime_FM_M_Ref``, respectively. User Guide ---------- -The user-configurable inputs to the profiler are the scalar maximum acceleration for the maneuver :math:`a_{\text{max}}`, -the mount frame axis for the translational motion, the prescribed body's initial position vector with respect to -the mount frame :math:`\boldsymbol{r}_{F/M}(t_0)`, and the reference position vector of the prescribed body with respect -to the mount frame :math:`\boldsymbol{r}_{F/M} (\text{ref})`. +The user-configurable inputs to the profiler are the scalar maximum acceleration for the translation +:math:`a_{\text{max}}`, the mount frame axis for the translational motion, the prescribed body's initial position +vector with respect to the mount frame :math:`\boldsymbol{r}_{F/M}(t_0)`, and the reference position vector of the +prescribed body with respect to the mount frame :math:`\boldsymbol{r}_{F/M} (\text{ref})`. -This module provides a :ref:`PrescribedMotionMsgPayload` output message that can be connected to the +This module provides a :ref:`PrescribedTranslationMsgPayload` output message that can be connected to the :ref:`PrescribedMotionStateEffector` dynamics module to directly profile a state effector's translational motion. +Note that a separate rotational profiler module can 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 translational module in python using Basilisk. @@ -150,9 +151,6 @@ This section is to outline the steps needed to setup a prescribed translational PrescribedTrans.r_FM_M = np.array([0.0, 0.0, 0.0]) PrescribedTrans.rPrime_FM_M = np.array([0.0, 0.0, 0.0]) PrescribedTrans.rPrimePrime_FM_M = np.array([0.0, 0.0, 0.0]) - PrescribedTrans.omega_FM_F = np.array([0.0, 0.0, 0.0]) - PrescribedTrans.omegaPrime_FM_F = np.array([0.0, 0.0, 0.0]) - PrescribedTrans.sigma_FM = np.array([0.0, 0.0, 0.0]) The user is required to set the above configuration data parameters, as they are not initialized in the module. From 2b95653b617b960abc38054d35cc4fed9ffb3788 Mon Sep 17 00:00:00 2001 From: Leah Kiner Date: Fri, 9 Feb 2024 16:00:29 -0700 Subject: [PATCH 4/7] Refactor prescribedRot1DOF module output message The module output message is changed from the prescribedMotionMsgPayload message containing both the translational and rotational states of a prescribed body to only the rotational states via the new prescribedRotationMsgPayload message --- .../_UnitTest/test_prescribedRot1DOF.py | 17 ++-- .../prescribedRot1DOF/prescribedRot1DOF.c | 24 ++--- .../prescribedRot1DOF/prescribedRot1DOF.h | 11 +-- .../prescribedRot1DOF/prescribedRot1DOF.i | 4 +- .../prescribedRot1DOF/prescribedRot1DOF.rst | 92 +++++++++---------- 5 files changed, 66 insertions(+), 82 deletions(-) diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/_UnitTest/test_prescribedRot1DOF.py b/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/_UnitTest/test_prescribedRot1DOF.py index 1407ac9622..0637e9e035 100755 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/_UnitTest/test_prescribedRot1DOF.py +++ b/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/_UnitTest/test_prescribedRot1DOF.py @@ -52,7 +52,7 @@ def test_prescribedRot1DOFTestFunction(show_plots, thetaInit, thetaRef, thetaDDo r""" **Validation Test Description** - This unit test ensures that the profiled 1 DOF attitude maneuver for a secondary rigid body connected + 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. @@ -67,8 +67,8 @@ def test_prescribedRot1DOFTestFunction(show_plots, thetaInit, thetaRef, thetaDDo **Description of Variables Being Tested** - This unit test ensures that the profiled 1 DOF rotational attitude maneuver is properly computed for a series of - initial and reference PRV angles and maximum angular accelerations. The final prescribed angle ``theta_FM_Final`` + 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. """ @@ -79,8 +79,8 @@ def test_prescribedRot1DOFTestFunction(show_plots, thetaInit, thetaRef, thetaDDo def prescribedRot1DOFTestFunction(show_plots, thetaInit, thetaRef, thetaDDotMax, accuracy): """Call this routine directly to run the unit test.""" - testFailCount = 0 # Zero the unit test result counter - testMessages = [] # Create an empty array to store the test log messages + testFailCount = 0 + testMessages = [] unitTaskName = "unitTask" unitProcessName = "TestProcess" bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) @@ -89,7 +89,7 @@ def prescribedRot1DOFTestFunction(show_plots, thetaInit, thetaRef, thetaDDotMax, unitTestSim = SimulationBaseClass.SimBaseClass() # Create the test thread - testProcessRate = macros.sec2nano(0.1) # update process rate update time + testProcessRate = macros.sec2nano(0.1) testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -103,9 +103,6 @@ def prescribedRot1DOFTestFunction(show_plots, thetaInit, thetaRef, thetaDDotMax, # Initialize the prescribedRot1DOF test module configuration data rotAxisM = np.array([1.0, 0.0, 0.0]) prvInit_FM = thetaInit * rotAxisM - PrescribedRot1DOF.r_FM_M = np.array([1.0, 0.0, 0.0]) - PrescribedRot1DOF.rPrime_FM_M = np.array([0.0, 0.0, 0.0]) - PrescribedRot1DOF.rPrimePrime_FM_M = np.array([0.0, 0.0, 0.0]) PrescribedRot1DOF.rotAxis_M = rotAxisM PrescribedRot1DOF.thetaDDotMax = thetaDDotMax PrescribedRot1DOF.omega_FM_F = np.array([0.0, 0.0, 0.0]) @@ -121,7 +118,7 @@ def prescribedRot1DOFTestFunction(show_plots, thetaInit, thetaRef, thetaDDotMax, PrescribedRot1DOF.spinningBodyInMsg.subscribeTo(HingedRigidBodyMessage) # Log the test module output message for data comparison - dataLog = PrescribedRot1DOF.prescribedMotionOutMsg.recorder() + dataLog = PrescribedRot1DOF.prescribedRotationOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) # Initialize the simulation diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.c b/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.c index 3969f31d8a..4c546b4c9a 100755 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.c +++ b/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.c @@ -35,7 +35,7 @@ void SelfInit_prescribedRot1DOF(PrescribedRot1DOFConfig *configData, int64_t moduleID) { // Initialize the output messages - PrescribedMotionMsg_C_init(&configData->prescribedMotionOutMsg); + PrescribedRotationMsg_C_init(&configData->prescribedRotationOutMsg); HingedRigidBodyMsg_C_init(&configData->spinningBodyOutMsg); } @@ -74,11 +74,11 @@ void Update_prescribedRot1DOF(PrescribedRot1DOFConfig *configData, uint64_t call // Create the buffer messages HingedRigidBodyMsgPayload spinningBodyIn; HingedRigidBodyMsgPayload spinningBodyOut; - PrescribedMotionMsgPayload prescribedMotionOut; + PrescribedRotationMsgPayload prescribedRotationOut; // Zero the output messages spinningBodyOut = HingedRigidBodyMsg_C_zeroMsgPayload(); - prescribedMotionOut = PrescribedMotionMsg_C_zeroMsgPayload(); + prescribedRotationOut = PrescribedRotationMsg_C_zeroMsgPayload(); // Read the input message spinningBodyIn = HingedRigidBodyMsg_C_zeroMsgPayload(); @@ -146,11 +146,6 @@ void Update_prescribedRot1DOF(PrescribedRot1DOFConfig *configData, uint64_t call configData->convergence = true; } - // Determine the prescribed parameters: omega_FM_F and omegaPrime_FM_F - v3Normalize(configData->rotAxis_M, configData->rotAxis_M); - v3Scale(thetaDot, configData->rotAxis_M, configData->omega_FM_F); - v3Scale(thetaDDot, configData->rotAxis_M, configData->omegaPrime_FM_F); - // Determine dcm_FF0 double dcm_FF0[3][3]; double prv_FF0_array[3]; @@ -171,13 +166,10 @@ void Update_prescribedRot1DOF(PrescribedRot1DOFConfig *configData, uint64_t call // Determine the prescribed parameter: sigma_FM C2MRP(dcm_FM, configData->sigma_FM); - // Copy the module variables to the prescribedMotionOut output message - v3Copy(configData->r_FM_M, prescribedMotionOut.r_FM_M); - v3Copy(configData->rPrime_FM_M, prescribedMotionOut.rPrime_FM_M); - v3Copy(configData->rPrimePrime_FM_M, prescribedMotionOut.rPrimePrime_FM_M); - v3Copy(configData->omega_FM_F, prescribedMotionOut.omega_FM_F); - v3Copy(configData->omegaPrime_FM_F, prescribedMotionOut.omegaPrime_FM_F); - v3Copy(configData->sigma_FM, prescribedMotionOut.sigma_FM); + // Copy the module variables to the prescribedRotationOut output message + v3Copy(configData->omega_FM_F, prescribedRotationOut.omega_FM_F); + v3Copy(configData->omegaPrime_FM_F, prescribedRotationOut.omegaPrime_FM_F); + v3Copy(configData->sigma_FM, prescribedRotationOut.sigma_FM); // Copy the local scalar variables to the spinningBodyOut output message spinningBodyOut.theta = theta; @@ -185,5 +177,5 @@ void Update_prescribedRot1DOF(PrescribedRot1DOFConfig *configData, uint64_t call // Write the output messages HingedRigidBodyMsg_C_write(&spinningBodyOut, &configData->spinningBodyOutMsg, moduleID, callTime); - PrescribedMotionMsg_C_write(&prescribedMotionOut, &configData->prescribedMotionOutMsg, moduleID, callTime); + PrescribedRotationMsg_C_write(&prescribedRotationOut, &configData->prescribedRotationOutMsg, moduleID, callTime); } diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.h b/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.h index ec4b326915..a39bd6235f 100755 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.h +++ b/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.h @@ -23,7 +23,7 @@ #include #include "architecture/utilities/bskLogging.h" #include "cMsgCInterface/HingedRigidBodyMsg_C.h" -#include "cMsgCInterface/PrescribedMotionMsg_C.h" +#include "cMsgCInterface/PrescribedRotationMsg_C.h" /*! @brief Top level structure for the sub-module routines. */ typedef struct { @@ -31,9 +31,6 @@ typedef struct { /* 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 r_FM_M[3]; //!< [m] Position of the F frame origin with respect to the M frame origin in M frame components (fixed) - double rPrime_FM_M[3]; //!< [m/s] B frame time derivative of r_FM_M in M frame components (fixed) - double rPrimePrime_FM_M[3]; //!< [m/s^2] B frame time derivative of rPrime_FM_M in M frame components (fixed) 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 @@ -53,9 +50,9 @@ typedef struct { BSKLogger *bskLogger; //!< BSK Logging /* Messages */ - HingedRigidBodyMsg_C spinningBodyInMsg; //!< Input msg for the spinning body reference angle and angle rate - HingedRigidBodyMsg_C spinningBodyOutMsg; //!< Output msg for the spinning body angle and angle rate - PrescribedMotionMsg_C prescribedMotionOutMsg; //!< Output msg for the spinning body prescribed states + HingedRigidBodyMsg_C spinningBodyInMsg; //!< Input msg for the spinning body reference angle and angle rate + HingedRigidBodyMsg_C spinningBodyOutMsg; //!< Output msg for the spinning body angle and angle rate + PrescribedRotationMsg_C prescribedRotationOutMsg; //!< Output msg for the spinning body prescribed rotational states }PrescribedRot1DOFConfig; diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.i b/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.i index 133669f79c..b7434d0a85 100755 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.i +++ b/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.i @@ -26,8 +26,8 @@ %include "architecture/msgPayloadDefC/HingedRigidBodyMsgPayload.h" struct HingedRigidBodyMsg_C; -%include "architecture/msgPayloadDefC/PrescribedMotionMsgPayload.h" -struct PrescribedMotionMsg_C; +%include "architecture/msgPayloadDefC/PrescribedRotationMsgPayload.h" +struct PrescribedRotationMsg_C; %pythoncode %{ import sys diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.rst b/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.rst index 644f7382a3..01f07cde2a 100644 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.rst +++ b/src/fswAlgorithms/effectorInterfaces/prescribedRot1DOF/prescribedRot1DOF.rst @@ -1,24 +1,24 @@ Executive Summary ----------------- -This module profiles a :ref:`PrescribedMotionMsgPayload` message for a specified 1 DOF rotational attitude maneuver -for a secondary rigid body connected to a rigid spacecraft hub at a hub-fixed location, :math:`\mathcal{M}`. The body +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 are: ``r_FM_M``, -``rPrime_FM_M``, ``rPrimePrime_FM_M``, ``omega_FM_F``, ``omegaPrime_FM_F``, and ``sigma_FM``. Because this is a -purely rotational profiler, the translational states ``r_FM_M``, ``rPrime_FM_M``, and ``rPrimePrime_FM_M`` are held -constant in this module. - -To use this module for prescribed motion purposes, it must be connected to the :ref:`PrescribedMotionStateEffector` -dynamics module in order to profile the states of the secondary body. The required maneuver is determined from the -user-specified scalar maximum angular acceleration for the attitude maneuver :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 maneuver and -constant negatively for the second half of the maneuver. The resulting angular velocity of the prescribed body is -linear, approaching a maximum magnitude halfway through the maneuver and ending with zero residual velocity. -The corresponding angle the prescribed body moves through during the maneuver is parabolic in time. +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. Message Connection Descriptions ------------------------------- @@ -36,27 +36,27 @@ provides information on what this message is used for. - Description * - spinningBodyInMsg - :ref:`HingedRigidBodyMsgPayload` - - input msg with the spinning body reference states + - input msg with the scalar spinning body rotational reference states * - spinningBodyOutMsg - :ref:`HingedRigidBodyMsgPayload` - - output message with the scalar spinning body states + - output message with the profiled scalar spinning body rotational states * - prescribedMotionOutMsg - - :ref:`PrescribedMotionMsgPayload` - - output message with the prescribed spinning body states + - :ref:`PrescribedRotationMsgPayload` + - output message with the profiled prescribed spinning body rotational states Detailed Module Description --------------------------- -This 1 DOF rotational motion flight software 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 attitude maneuver +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 attitude maneuver. +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 maneuver: +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 ) @@ -64,22 +64,22 @@ rotation angle and axis for the maneuver: .. 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 attitude maneuver, the prescribed body is constantly accelerated with the given maximum +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 attitude maneuver. The switch time :math:`t_s` is the simulation time -halfway through the maneuver: +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 maneuver :math:`\Delta t` is determined using the inputs to the profiler: +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 maneuver is parabolic. The profiled +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 attitude maneuver +the profiled motion is instead concave downwards. The described motion during the first half of the rotation is characterized by the expressions: .. math:: @@ -96,11 +96,11 @@ where .. math:: c_1 = \frac{\Phi_{\text{ref}}}{2(t_s - t_0)^2} -Similarly, the second half of the attitude maneuver decelerates the prescribed body constantly until it reaches a +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 maneuver 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 attitude maneuver is characterized by the expressions: +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}} @@ -118,23 +118,24 @@ The described motion during the second half of the attitude maneuver is characte Module Testing ^^^^^^^^^^^^^^ -The unit test for this module ensures that the profiled 1 DOF rotational attitude maneuver is properly computed for a series of +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 attitude maneuver +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:`PrescribedMotionMsgPayload`. The first guidance message, describing the spinning body's scalar states relative to -the body-fixed mount frame can be directly connected to an attitude feedback control module. The second prescribed -motion output message can be connected to the :ref:`PrescribedMotionStateEffector` dynamics module to directly profile -a state effector's rotational motion. +: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. @@ -152,9 +153,6 @@ This section is to outline the steps needed to setup a prescribed 1 DOF rotation thetaInit = 0.0 # [rad] rotAxis_M = np.array([1.0, 0.0, 0.0]) prvInit_FM = thetaInit * rotAxisM - PrescribedRot1DOF.r_FM_M = np.array([1.0, 0.0, 0.0]) - PrescribedRot1DOF.rPrime_FM_M = np.array([0.0, 0.0, 0.0]) - PrescribedRot1DOF.rPrimePrime_FM_M = np.array([0.0, 0.0, 0.0]) PrescribedRot1DOF.rotAxis_M = rotAxis_M PrescribedRot1DOF.thetaDDotMax = 0.01 # [rad/s^2] PrescribedRot1DOF.omega_FM_F = np.array([0.0, 0.0, 0.0]) From f5fee0819268cb1d2cc17569e0361ced118f60b5 Mon Sep 17 00:00:00 2001 From: Leah Kiner Date: Fri, 9 Feb 2024 16:04:57 -0700 Subject: [PATCH 5/7] Refactor prescribedRot2DOF module output message The module output message is changed from the prescribedMotionMsgPayload message containing both the translational and rotational states of a prescribed body to only the rotational states via the new prescribedRotationMsgPayload message --- .../_UnitTest/test_prescribedRot2DOF.py | 55 ++++++------ .../prescribedRot2DOF/prescribedRot2DOF.c | 21 ++--- .../prescribedRot2DOF/prescribedRot2DOF.h | 11 +-- .../prescribedRot2DOF/prescribedRot2DOF.i | 4 +- .../prescribedRot2DOF/prescribedRot2DOF.rst | 90 +++++++++---------- 5 files changed, 84 insertions(+), 97 deletions(-) diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/_UnitTest/test_prescribedRot2DOF.py b/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/_UnitTest/test_prescribedRot2DOF.py index 261900d394..5b39a97d38 100755 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/_UnitTest/test_prescribedRot2DOF.py +++ b/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/_UnitTest/test_prescribedRot2DOF.py @@ -44,18 +44,18 @@ # Parametrize the user-configurable variables @pytest.mark.parametrize("thetaInit", [0.01]) -@pytest.mark.parametrize("thetaRef1a", [0.0, 2*np.pi/3]) # Maneuver 1 -@pytest.mark.parametrize("thetaRef2a", [np.pi/3, 2*np.pi/3]) # Maneuver 1 -@pytest.mark.parametrize("thetaRef1b", [0.0, 2*np.pi/3]) # Maneuver 2 -@pytest.mark.parametrize("thetaRef2b", [np.pi/3, 2*np.pi/3]) # Maneuver 2 +@pytest.mark.parametrize("thetaRef1a", [0.0, 2*np.pi/3]) # Rotation 1 +@pytest.mark.parametrize("thetaRef2a", [np.pi/3, 2*np.pi/3]) # Rotation 1 +@pytest.mark.parametrize("thetaRef1b", [0.0, 2*np.pi/3]) # Rotation 2 +@pytest.mark.parametrize("thetaRef2b", [np.pi/3, 2*np.pi/3]) # Rotation 2 @pytest.mark.parametrize("phiDDotMax", [0.004]) @pytest.mark.parametrize("accuracy", [1e-5]) def test_PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, thetaRef1b, thetaRef2b, phiDDotMax, accuracy): r""" **Validation Test Description** - The unit test for this module simulates TWO consecutive 2 DOF rotational attitude maneuvers for a secondary rigid - body connected to a rigid spacecraft hub. Two maneuvers are simulated to ensure that the module correctly updates + The unit test for this module simulates TWO consecutive 2 DOF rotations for a secondary rigid body connected + to a rigid spacecraft hub. Two rotations are simulated to ensure that the module correctly updates the required relative PRV attitude when a new attitude reference message is written. This unit test checks that the prescribed body's MRP attitude converges to both reference attitudes for a series of initial and reference attitudes and maximum angular accelerations. (``sigma_FM_Final1`` is checked to converge to ``sigma_FM_Ref1``, and @@ -67,18 +67,18 @@ def test_PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaR Args: thetaInit (float): [rad] Initial PRV angle of the F frame with respect to the M frame - thetaRef1a (float): [rad] First reference PRV angle for the first attitude maneuver - thetaRef2a (float): [rad] Second reference PRV angle for the first attitude maneuver - thetaRef1b (float): [rad] First reference PRV angle for the second attitude maneuver - thetaRef2b (float): [rad] Second reference PRV angle for the second attitude maneuver - phiDDotMax (float): [rad/s^2] Maximum angular acceleration for the attitude maneuver + thetaRef1a (float): [rad] First reference PRV angle for the first rotation + thetaRef2a (float): [rad] Second reference PRV angle for the first rotation + thetaRef1b (float): [rad] First reference PRV angle for the second rotation + thetaRef2b (float): [rad] Second reference PRV angle for the second rotation + phiDDotMax (float): [rad/s^2] Maximum angular acceleration for the rotation accuracy (float): absolute accuracy value used in the validation tests **Description of Variables Being Tested** - The prescribed body's MRP attitude at the end of the first maneuver ``sigma_FM_Final1`` is checked to converge to + The prescribed body's MRP attitude at the end of the first rotation ``sigma_FM_Final1`` is checked to converge to the first reference attitude ``sigma_FM_Ref1``. The prescribed body's MRP attitude at the end of the second - maneuver ``sigma_FM_Final2`` is checked to converge to the second reference attitude ``sigma_FM_Ref2``. + rotation ``sigma_FM_Final2`` is checked to converge to the second reference attitude ``sigma_FM_Ref2``. Additionally, the prescribed body's final angular velocity magnitude ``thetaDot_Final`` is checked for convergence to the reference angular velocity magnitude, ``thetaDot_Ref``. """ @@ -90,8 +90,8 @@ def test_PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaR def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, thetaRef1b, thetaRef2b, phiDDotMax, accuracy): """Call this routine directly to run the unit test.""" - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages + testFailCount = 0 + testMessages = [] unitTaskName = "unitTask" unitProcessName = "TestProcess" bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) @@ -114,9 +114,6 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, prescribedRot2DOFObj.rotAxis1_M = rotAxis1_M prescribedRot2DOFObj.rotAxis2_F1 = rotAxis2_F1 prescribedRot2DOFObj.phiDDotMax = phiDDotMax - prescribedRot2DOFObj.r_FM_M = np.array([1.0, 0.0, 0.0]) # [m] Position of the F frame origin relative to the M frame origin in M frame components - prescribedRot2DOFObj.rPrime_FM_M = np.array([0.0, 0.0, 0.0]) # [m/s] B frame time derivative of r_FM_M in M frame components - prescribedRot2DOFObj.rPrimePrime_FM_M = np.array([0.0, 0.0, 0.0]) # [m/s^2] B frame time derivative of rPrime_FM_M in M frame components prescribedRot2DOFObj.omega_FM_F = np.array([0.0, 0.0, 0.0]) # [rad/s] Angular velocity of frame F relative to frame M in F frame components prescribedRot2DOFObj.omegaPrime_FM_F = np.array([0.0, 0.0, 0.0]) # [rad/s^2] B frame time derivative of omega_FB_F in F frame components prescribedRot2DOFObj.sigma_FM = np.array([0.0, 0.0, 0.0]) # MRP attitude of frame F relative to frame M @@ -138,7 +135,7 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, prescribedRot2DOFObj.spinningBodyRef2InMsg.subscribeTo(HingedRigidBodyMessage2) # Set up message data recording logging on the test module output message to get access to it - dataLog = prescribedRot2DOFObj.prescribedMotionOutMsg.recorder() + dataLog = prescribedRot2DOFObj.prescribedRotationOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) # Set up module variable data recording @@ -148,11 +145,11 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, # Initialize the simulation unitTestSim.InitializeSimulation() - # Calculate the two reference PRVs for the first attitude maneuver + # Calculate the two reference PRVs for the first rotation prv_F0M_a = thetaRef1a * rotAxis1_M[0], thetaRef1a * rotAxis1_M[1], thetaRef1a * rotAxis1_M[2] prv_F1F0_a = thetaRef2a * rotAxis2_F1[0], thetaRef2a * rotAxis2_F1[1], thetaRef2a * rotAxis2_F1[2] - # Calculate a single reference PRV for the first attitude maneuver and the associated MRP attitude + # Calculate a single reference PRV for the first rotation and the associated MRP attitude if (thetaRef1a == 0 and thetaRef2a == 0): # Prevent a (0,0,0) error using rbk.addPRV() prv_F1M_a = np.array([0.0, 0.0, 0.0]) phi_F1M_a = 0.0 @@ -162,28 +159,28 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, phi_F1M_a = np.linalg.norm(prv_F1M_a) sigma_FM_Ref1 = rbk.PRV2MRP(prv_F1M_a) - # Set the simulation time for the first attitude maneuver + # Set the simulation time for the first rotation simTime1 = np.sqrt(((0.5 * np.abs(phi_F1M_a)) * 8) / phiDDotMax) + 10 unitTestSim.ConfigureStopTime(macros.sec2nano(simTime1)) - # Execute the first attitude maneuver + # Execute the first rotation unitTestSim.ExecuteSimulation() # Extract the logged sigma_FM MRPs for data comparison sigma_FM_FirstMan = dataLog.sigma_FM sigma_FM_Final1 = sigma_FM_FirstMan[-1, :] - # Calculate the two reference PRVs for the second attitude maneuver + # Calculate the two reference PRVs for the second rotation prv_F2M_b = thetaRef1b * rotAxis1_M[0], thetaRef1b * rotAxis1_M[1], thetaRef1b * rotAxis1_M[2] prv_F3F2_b = thetaRef2b * rotAxis2_F1[0], thetaRef2b * rotAxis2_F1[1], thetaRef2b * rotAxis2_F1[2] - # Calculate a single reference PRV (prv_F3M_b) for the second attitude maneuver beginning from the M frame + # Calculate a single reference PRV (prv_F3M_b) for the second rotation beginning from the M frame if (thetaRef1b == 0 and thetaRef2b == 0): # Prevent a (0,0,0) error using rbk.addPRV() prv_F3M_b = np.array([0.0, 0.0, 0.0]) else: prv_F3M_b = rbk.addPRV(prv_F2M_b, prv_F3F2_b) - # Calculate a single reference PRV (prv_F3F1_b) for the second attitude maneuver beginning from the spinning body location after the first maneuver (F1) + # Calculate a single reference PRV (prv_F3F1_b) for the second rotation beginning from the spinning body location after the first rotation (F1) # Also calculate the MRP representing the desired final attitude of the spinning body with respesct to the M frame if not unitTestSupport.isArrayEqual(prv_F1M_a, prv_F3M_b, 3, 1e-12): prv_F3F1_b = rbk.subPRV(prv_F1M_a, prv_F3M_b) @@ -193,7 +190,7 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, sigma_FM_Ref2 = sigma_FM_Ref1 phi_F3F1_b = np.linalg.norm(prv_F3F1_b) - # Write the HingedRigidBody reference messages for the second maneuver + # Write the HingedRigidBody reference messages for the second rotation hingedRigidBodyMessageData1 = messaging.HingedRigidBodyMsgPayload() hingedRigidBodyMessageData2 = messaging.HingedRigidBodyMsgPayload() hingedRigidBodyMessageData1.theta = thetaRef1b @@ -205,11 +202,11 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, prescribedRot2DOFObj.spinningBodyRef1InMsg.subscribeTo(HingedRigidBodyMessage1) prescribedRot2DOFObj.spinningBodyRef2InMsg.subscribeTo(HingedRigidBodyMessage2) - # Set the simulation time for the second maneuver + # Set the simulation time for the second rotation simTime2 = np.sqrt(((0.5 * np.abs(phi_F3F1_b)) * 8) / phiDDotMax) + 10 unitTestSim.ConfigureStopTime(macros.sec2nano(simTime1 + simTime2)) - # Execute the second attitude maneuver + # Execute the second rotation unitTestSim.ExecuteSimulation() # Extract the recorded data for data comparison and plotting diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.c b/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.c index be2df27678..865ac1b0b0 100755 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.c +++ b/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.c @@ -33,7 +33,7 @@ */ void SelfInit_prescribedRot2DOF(PrescribedRot2DOFConfig *configData, int64_t moduleID) { - PrescribedMotionMsg_C_init(&configData->prescribedMotionOutMsg); + PrescribedRotationMsg_C_init(&configData->prescribedRotationOutMsg); } /*! This method performs a complete reset of the module. Local module variables that retain @@ -93,11 +93,11 @@ void Update_prescribedRot2DOF(PrescribedRot2DOFConfig *configData, uint64_t call HingedRigidBodyMsgPayload spinningBodyRef1In; HingedRigidBodyMsgPayload spinningBodyRef2In; HingedRigidBodyMsgPayload spinningBodyOut; - PrescribedMotionMsgPayload prescribedMotionOut; + PrescribedRotationMsgPayload prescribedRotationOut; // Zero the output messages spinningBodyOut = HingedRigidBodyMsg_C_zeroMsgPayload(); - prescribedMotionOut = PrescribedMotionMsg_C_zeroMsgPayload(); + prescribedRotationOut = PrescribedRotationMsg_C_zeroMsgPayload(); // Read the input messages spinningBodyRef1In = HingedRigidBodyMsg_C_zeroMsgPayload(); @@ -230,14 +230,11 @@ void Update_prescribedRot2DOF(PrescribedRot2DOFConfig *configData, uint64_t call // Determine the prescribed spinning body state: sigma_FM C2MRP(dcm_FM, configData->sigma_FM); - // Copy the module prescribed variables to the prescribed motion output message - v3Copy(configData->r_FM_M, prescribedMotionOut.r_FM_M); - v3Copy(configData->rPrime_FM_M, prescribedMotionOut.rPrime_FM_M); - v3Copy(configData->rPrimePrime_FM_M, prescribedMotionOut.rPrimePrime_FM_M); - v3Copy(configData->omega_FM_F, prescribedMotionOut.omega_FM_F); - v3Copy(configData->omegaPrime_FM_F, prescribedMotionOut.omegaPrime_FM_F); - v3Copy(configData->sigma_FM, prescribedMotionOut.sigma_FM); + // Copy the module prescribed variables to the prescribed rotational motion output message + v3Copy(configData->omega_FM_F, prescribedRotationOut.omega_FM_F); + v3Copy(configData->omegaPrime_FM_F, prescribedRotationOut.omegaPrime_FM_F); + v3Copy(configData->sigma_FM, prescribedRotationOut.sigma_FM); - // Write the prescribed motion output message - PrescribedMotionMsg_C_write(&prescribedMotionOut, &configData->prescribedMotionOutMsg, moduleID, callTime); + // Write the prescribed rotational motion output message + PrescribedRotationMsg_C_write(&prescribedRotationOut, &configData->prescribedRotationOutMsg, moduleID, callTime); } diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.h b/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.h index ba4587d44e..d5da1a5bda 100755 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.h +++ b/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.h @@ -24,7 +24,7 @@ #include #include "architecture/utilities/bskLogging.h" #include "cMsgCInterface/HingedRigidBodyMsg_C.h" -#include "cMsgCInterface/PrescribedMotionMsg_C.h" +#include "cMsgCInterface/PrescribedRotationMsg_C.h" /*! @brief Top level structure for the sub-module routines. */ typedef struct @@ -35,9 +35,6 @@ typedef struct double rotAxis2_F1[3]; //!< F1 frame intermediate rotation axis for the second rotation /* Private variables */ - double r_FM_M[3]; //!< [m] Position of the F frame origin relative to the M frame origin in M frame components - double rPrime_FM_M[3]; //!< [m/s] B frame time derivative of r_FM_M in M frame components - double rPrimePrime_FM_M[3]; //!< [m/s^2] B frame time derivative of rPrime_FM_M in M frame components double omega_FM_F[3]; //!< [rad/s] angular velocity of frame F relative to frame M in F frame components double omegaPrime_FM_F[3]; //!< [rad/s^2] B frame time derivative of omega_FB_F in F frame components double sigma_FM[3]; //!< MRP attitude of frame F relative to frame M @@ -56,9 +53,9 @@ typedef struct double dcm_F0M[3][3]; //!< DCM from the M frame to the spinning body body frame at the beginning of the maneuver /* Declare the module input-output messages */ - HingedRigidBodyMsg_C spinningBodyRef1InMsg; //!< Input msg for the first reference angle and angle rate - HingedRigidBodyMsg_C spinningBodyRef2InMsg; //!< Input msg for the second reference angles and angle rate - PrescribedMotionMsg_C prescribedMotionOutMsg; //!< Output msg for the profiled prescribed states + HingedRigidBodyMsg_C spinningBodyRef1InMsg; //!< Input msg for the first reference angle and angle rate + HingedRigidBodyMsg_C spinningBodyRef2InMsg; //!< Input msg for the second reference angles and angle rate + PrescribedRotationMsg_C prescribedRotationOutMsg; //!< Output msg for the profiled prescribed rotational states BSKLogger *bskLogger; //!< BSK Logging diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.i b/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.i index f88ffe7a69..bbd1ddcd56 100755 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.i +++ b/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.i @@ -27,8 +27,8 @@ %include "architecture/msgPayloadDefC/HingedRigidBodyMsgPayload.h" struct HingedRigidBodyMsg_C; -%include "architecture/msgPayloadDefC/PrescribedMotionMsgPayload.h" -struct PrescribedMotionMsg_C; +%include "architecture/msgPayloadDefC/PrescribedRotationMsgPayload.h" +struct PrescribedRotationMsg_C; %pythoncode %{ import sys diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.rst b/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.rst index b0992c0c3a..33a67cefe0 100644 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.rst +++ b/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/prescribedRot2DOF.rst @@ -1,31 +1,29 @@ Executive Summary ----------------- -This module profiles a :ref:`PrescribedMotionMsgPayload` message for a specified 2 DOF rotational attitude maneuver +This module profiles a :ref:`PrescribedRotationMsgPayload` message for a specified 2 DOF rotation for a secondary 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 are: ``r_FM_M``, -``rPrime_FM_M``, ``rPrimePrime_FM_M``, ``omega_FM_F``, ``omegaPrime_FM_F``, and ``sigma_FM``. Because this is a -purely rotational profiler, the translational states ``r_FM_M``, ``rPrime_FM_M``, and ``rPrimePrime_FM_M`` are held -constant in this module. +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``. It should be noted that although the inputs to this module are two consecutive rotation angles and axes, the resulting -maneuver that is profiled is a 1 DOF rotational attitude maneuver. The module converts two given reference angles and -their corresponding rotation axes for the attitude maneuver to a single 1 DOF rotation for the maneuver. -Simple Principal Rotation Vector (PRV) addition is used on the two given reference PRVs to determine the single PRV -required for the attitude maneuver. - -To use this module for prescribed motion purposes, it must be connected to the :ref:`PrescribedMotionStateEffector` -dynamics module in order to profile the states of the secondary body. The required maneuver is determined from the -user-specified scalar maximum angular acceleration for the attitude maneuver, :math:`\alpha_{\text{max}}`, the spinning -body's initial attitude with respect to the mount frame as the Principal Rotation Vector ``prv_F0M`` -:math:`(\Phi_0, \hat{\boldsymbol{e}}_0)`, and two reference Principal Rotation Vectors for the attitude maneuver, -``prv_F1F0`` :math:`(\Phi_{1a}, \hat{\boldsymbol{e}}_{1a})` and ``prv_F2F1`` +rotation that is profiled is a 1 DOF rotation. The module converts two given reference angles and their corresponding +rotation axes for the rotation to a single 1 DOF rotation for the rotation. Simple Principal Rotation Vector (PRV) +addition is used on the two given reference PRVs to determine the single PRV required for the rotation. + +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}}`, the spinning body's initial attitude with respect to the mount frame as the Principal +Rotation Vector ``prv_F0M`` :math:`(\Phi_0, \hat{\boldsymbol{e}}_0)`, and two reference Principal Rotation Vectors +for the rotation, ``prv_F1F0`` :math:`(\Phi_{1a}, \hat{\boldsymbol{e}}_{1a})` and ``prv_F2F1`` :math:`(\Phi_{1b}, \hat{\boldsymbol{e}}_{1b})`. -The maximum scalar angular acceleration is applied constant and positively for the first half of the maneuver and -constant negatively for the second half of the maneuver. The resulting angular velocity of the prescribed body is -linear, approaching a maximum magnitude halfway through the maneuver and ending with zero residual velocity. -The corresponding angle the prescribed body moves through during the maneuver is parabolic in time. +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. Message Connection Descriptions ------------------------------- @@ -43,23 +41,23 @@ provides information on what this message is used for. - Description * - spinningBodyRef1InMsg - :ref:`HingedRigidBodyMsgPayload` - - input msg with the spinning body reference states for the maneuver's first rotation + - input msg with the scalar spinning body rotational reference states for the first rotation * - spinningBodyRef2InMsg - :ref:`HingedRigidBodyMsgPayload` - - input msg with the spinning body reference states for the maneuver's second rotation + - input msg with the scalar spinning body rotational reference states for the second rotation * - prescribedMotionOutMsg - - :ref:`PrescribedMotionMsgPayload` - - output message with the prescribed spinning body states + - :ref:`PrescribedRotationMsgPayload` + - output message with the prescribed spinning body rotational states Detailed Module Description --------------------------- -This 2 DOF rotational motion flight software module converts a given 2 DOF attitude maneuver to a single 1 DOF attitude -maneuver and profiles the required spinning body rotational motion with respect to a body-fixed mount frame for the -maneuver. The inputs to the profiler are the maximum angular acceleration for the attitude maneuver, +This 2 DOF rotational motion kinematic profiler module converts a given 2 DOF rotation to a single 1 DOF rotation +and profiles the required spinning body rotational motion with respect to a body-fixed mount frame for the +rotation. The inputs to the profiler are the maximum angular acceleration for the rotation, :math:`\alpha_{\text{max}}`, the spinning body's initial attitude with respect to the mount frame as the Principal Rotation Vector ``prv_F0M`` :math:`(\Phi_0, \hat{\boldsymbol{e}}_0)`, and two reference Principal Rotation Vectors -for the attitude maneuver, ``prv_F1F0`` :math:`(\Phi_{1a}, \hat{\boldsymbol{e}}_{1a})` and ``prv_F2F1`` +for the rotation, ``prv_F1F0`` :math:`(\Phi_{1a}, \hat{\boldsymbol{e}}_{1a})` and ``prv_F2F1`` :math:`(\Phi_{1b}, \hat{\boldsymbol{e}}_{1b})`. The module first converts the two given reference PRVs to a single PRV, ``prv_F2M`` that represents the final spinning @@ -72,7 +70,7 @@ body attitude with respect to the body-fixed mount frame: \hat{\boldsymbol{e}}_2 = \frac{\cos \frac{\Phi_{1b}}{2} \sin \frac{\Phi_{1a}}{2} \ \hat{\boldsymbol{e}}_{1a} + \cos \frac{\Phi_{1a}}{2} \sin \frac{\Phi_{1b}}{2} \ \boldsymbol{e}_{1b} + \sin \frac{\Phi_{1a}}{2} \sin \frac{\Phi_{1b}}{2} \ \hat{\boldsymbol{e}}_{1a} \times \hat{\boldsymbol{e}}_{1b} }{\sin \frac{\Phi_2}{2}} Subtracting the initial Principal Rotation Vector ``prv_F0M`` from the found reference PRV ``prv_F2M`` gives the -required PRV for the maneuver, ``prv_F2F0``: +required PRV for the rotation, ``prv_F2F0``: .. math:: \Phi_{\text{ref}} = \Delta \Phi = 2 \cos^{-1} \left ( \cos \frac{\Phi_2}{2} \cos \frac{\Phi_0}{2} + \sin \frac{\Phi_2}{2} \sin \frac {\Phi_0}{2} \ \hat{\boldsymbol{e}}_2 \cdot \hat{\boldsymbol{e}}_0 \right ) @@ -80,26 +78,26 @@ required PRV for the maneuver, ``prv_F2F0``: .. math:: \hat{\boldsymbol{e}}_3 = \frac{\cos \frac{\Phi_0}{2} \sin \frac{\Phi_2}{2} \ \hat{\boldsymbol{e}}_2 - \cos \frac{\Phi_2}{2} \sin \frac{\Phi_0}{2} \ \hat{\boldsymbol{e}}_0 + \sin \frac{\Phi_2}{2} \sin \frac{\Phi_0}{2} \ \hat{\boldsymbol{e}}_2 \times \hat{\boldsymbol{e}}_0 }{\sin \frac{\Delta \Phi}{2}} -Note that the initial PRV angle, :math:`\Phi_0` is reset to zero for consecutive attitude maneuvers so that the -reference PRV angle, :math:`\Phi_{\text{ref}}` is always taken as the full angle to be swept during the maneuver. +Note that the initial PRV angle, :math:`\Phi_0` is reset to zero for consecutive rotations so that the +reference PRV angle, :math:`\Phi_{\text{ref}}` is always taken as the full angle to be swept during the rotation. -During the first half of the attitude maneuver, the spinning body is constantly accelerated with the given maximum +During the first half of the rotation, the spinning body is constantly accelerated with the given maximum angular acceleration. The spinning body's angular velocity increases linearly during the acceleration phase and reaches -a maximum magnitude halfway through the attitude maneuver. The switch time, :math:`t_s` is the simulation time halfway -through the maneuver: +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 maneuver, :math:`\Delta t` is determined using the found PRV angle for the maneuver: +where the time required for the rotation, :math:`\Delta t` is determined using the found PRV angle for the rotation: .. math:: \Delta t = t_f - t_0 = 2\sqrt{ \Phi_{\text{ref}} / \alpha_{\text{max}}} -The resulting trajectory of the angle :math:`\Phi` swept during the first half of the maneuver is quadratic. The +The resulting trajectory of the angle :math:`\Phi` swept during the first half of the rotation is quadratic. The profiled motion is concave upwards if the reference angle, :math:`\Phi_{\text{ref}}` is greater than zero. If the reference angle is negative, the profiled motion is instead concave downwards. The described motion during the first -half of the attitude maneuver is characterized by the expressions: +half of the rotation is characterized by the expressions: .. math:: \ddot{\Phi}(t) = \alpha_{\text{max}} @@ -115,11 +113,11 @@ where .. math:: a = \frac{ \frac{1}{2} \Phi_{\text{ref}}}{(t_s - t_0)^2} -Similarly, the second half of the attitude maneuver decelerates the spinning body constantly until it reaches a +Similarly, the second half of the rotation decelerates the spinning body constantly until it reaches a non-rotating state. The spinning body's angular velocity decreases linearly from its maximum magnitude back to zero. -The trajectory swept during the second half of the maneuver is quadratic and concave downwards if the reference angle, +The trajectory swept during the second half of the rotation is quadratic and concave downwards if the reference angle, :math:`\Phi_{\text{ref}}` is greater than zero. If the reference angle is negative, the profiled motion is instead -concave upwards. The described motion during the second half of the attitude maneuver is characterized by the +concave upwards. The described motion during the second half of the rotation is characterized by the expressions: .. math:: @@ -151,15 +149,16 @@ velocity magnitude ``thetaDot_Final`` is checked for convergence to the referenc User Guide ---------- -The user-configurable inputs to the profiler are the maximum angular acceleration for the attitude maneuver, +The user-configurable inputs to the profiler are the maximum angular acceleration for the rotation, :math:`\alpha_{\text{max}}`, the spinning body's initial attitude with respect to the mount frame as the Principal Rotation Vector ``prv_F0M`` :math:`(\Phi_0, \hat{\boldsymbol{e}}_0)`, and two reference Principal Rotation Vectors for -the attitude maneuver, ``prv_F1F0`` :math:`(\Phi_{1a}, \hat{\boldsymbol{e}}_{1a})` and ``prv_F2F1`` +the rotation, ``prv_F1F0`` :math:`(\Phi_{1a}, \hat{\boldsymbol{e}}_{1a})` and ``prv_F2F1`` :math:`(\Phi_{1b}, \hat{\boldsymbol{e}}_{1b})`. -This module provides a single output message in the form of :ref:`prescribedMotionMsgPayload`. This prescribed +This module provides a single output message in the form of :ref:`prescribedRotationMsgPayload`. This prescribed motion output message can be connected to the :ref:`prescribedMotionStateEffector` dynamics module to directly profile -a state effector's rotational motion. +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 2 DOF rotational module in python using Basilisk. @@ -179,9 +178,6 @@ This section is to outline the steps needed to setup a prescribed 2 DOF rotation PrescribedRot2DOF.rotAxis1_M = rotAxis1_M PrescribedRot2DOF.rotAxis2_F1 = rotAxis2_F1 PrescribedRot2DOF.phiDDotMax = phiDDotMax - PrescribedRot2DOF.r_FM_M = np.array([1.0, 0.0, 0.0]) # [m] Position of the F frame origin relative to the M frame origin in M frame components - PrescribedRot2DOF.rPrime_FM_M = np.array([0.0, 0.0, 0.0]) # [m/s] B frame time derivative of r_FM_M in M frame components - PrescribedRot2DOF.rPrimePrime_FM_M = np.array([0.0, 0.0, 0.0]) # [m/s^2] B frame time derivative of rPrime_FM_M in M frame components PrescribedRot2DOF.omega_FM_F = np.array([0.0, 0.0, 0.0]) # [rad/s] Angular velocity of frame F relative to frame M in F frame components PrescribedRot2DOF.omegaPrime_FM_F = np.array([0.0, 0.0, 0.0]) # [rad/s^2] B frame time derivative of omega_FB_F in F frame components PrescribedRot2DOF.sigma_FM = np.array([0.0, 0.0, 0.0]) # MRP attitude of frame F relative to frame M From 814fb2b93ada84451494f14f8115a740d273da4b Mon Sep 17 00:00:00 2001 From: Leah Kiner Date: Fri, 9 Feb 2024 16:11:23 -0700 Subject: [PATCH 6/7] Refactor prescribedMotion module with the new msgs The prescribedMotion module is refactored to use the two separate prescribed motion state messages. --- .../test_PrescribedMotionStateEffector.py | 43 +++++------- .../prescribedMotionStateEffector.cpp | 65 ++++++++++++------- .../prescribedMotionStateEffector.h | 11 ++-- .../prescribedMotionStateEffector.i | 6 +- .../prescribedMotionStateEffector.rst | 52 ++++++++------- 5 files changed, 97 insertions(+), 80 deletions(-) diff --git a/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffector.py b/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffector.py index 4d17d15394..5424c3004b 100644 --- a/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffector.py +++ b/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffector.py @@ -178,10 +178,6 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI # Initialize the prescribedRot1DOF test module configuration data accelMax = 0.01 # [rad/s^2] - #accelMax = np.pi / 180 # [rad/s^2] - PrescribedRot1DOF.r_FM_M = r_FM_M - PrescribedRot1DOF.rPrime_FM_M = np.array([0.0, 0.0, 0.0]) - PrescribedRot1DOF.rPrimePrime_FM_M = np.array([0.0, 0.0, 0.0]) PrescribedRot1DOF.rotAxis_M = rotAxis_M PrescribedRot1DOF.thetaDDotMax = accelMax PrescribedRot1DOF.omega_FM_F = np.array([0.0, 0.0, 0.0]) @@ -196,8 +192,8 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI SpinningBodyMessage = messaging.HingedRigidBodyMsg().write(SpinningBodyMessageData) PrescribedRot1DOF.spinningBodyInMsg.subscribeTo(SpinningBodyMessage) - # Connect the PrescribedRot1DOF module's prescribedMotion output message to the prescribedMotion module's prescribedMotion input message - platform.prescribedMotionInMsg.subscribeTo(PrescribedRot1DOF.prescribedMotionOutMsg) + # Connect the PrescribedRot1DOF module's prescribedRotation output message to the prescribedMotion module's prescribedRotation input message + platform.prescribedRotationInMsg.subscribeTo(PrescribedRot1DOF.prescribedRotationOutMsg) # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() @@ -212,11 +208,9 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI # Add other states to log scStateData = scObject.scStateOutMsg.recorder() - prescribedStateData = platform.prescribedMotionOutMsg.recorder() - dataLog = PrescribedRot1DOF.prescribedMotionOutMsg.recorder() + prescribedRotStateData = platform.prescribedRotationOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, scStateData) - unitTestSim.AddModelToTask(unitTaskName, prescribedStateData) - unitTestSim.AddModelToTask(unitTaskName, dataLog) + unitTestSim.AddModelToTask(unitTaskName, prescribedRotStateData) # Initialize the simulation unitTestSim.InitializeSimulation() @@ -236,10 +230,10 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI omega_BN_B = scStateData.omega_BN_B r_BN_N = scStateData.r_BN_N sigma_BN = scStateData.sigma_BN - omega_FM_F = dataLog.omega_FM_F - omegaPrime_FM_F = dataLog.omegaPrime_FM_F - sigma_FM = dataLog.sigma_FM - timespan = dataLog.times() + omega_FM_F = prescribedRotStateData.omega_FM_F + omegaPrime_FM_F = prescribedRotStateData.omegaPrime_FM_F + sigma_FM = prescribedRotStateData.sigma_FM + timespan = prescribedRotStateData.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)) @@ -419,9 +413,6 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI PrescribedTrans.rPrimePrime_FM_M = np.array([0.0, 0.0, 0.0]) PrescribedTrans.transAxis_M = transAxis_M PrescribedTrans.scalarAccelMax = accelMax - PrescribedTrans.omega_FM_F = np.array([0.0, 0.0, 0.0]) - PrescribedTrans.omegaPrime_FM_F = np.array([0.0, 0.0, 0.0]) - PrescribedTrans.sigma_FM = sigma_FM # Create the prescribedTrans input message velRef = 0.0 # [m/s] @@ -431,8 +422,8 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI linearTranslationRigidBodyMessage = messaging.LinearTranslationRigidBodyMsg().write(linearTranslationRigidBodyMessageData) PrescribedTrans.linearTranslationRigidBodyInMsg.subscribeTo(linearTranslationRigidBodyMessage) - # Connect the PrescribedTrans module's prescribedMotion output message to the prescribedMotion module's prescribedMotion input message - platform.prescribedMotionInMsg.subscribeTo(PrescribedTrans.prescribedMotionOutMsg) + # Connect the PrescribedTrans module's prescribedTranslation output message to the prescribedMotion module's prescribedTranslation input message + platform.prescribedTranslationInMsg.subscribeTo(PrescribedTrans.prescribedTranslationOutMsg) # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() @@ -447,11 +438,9 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI # Add other states to log scStateData = scObject.scStateOutMsg.recorder() - prescribedStateData = platform.prescribedMotionOutMsg.recorder() - dataLog = PrescribedTrans.prescribedMotionOutMsg.recorder() + prescribedTransStateData = platform.prescribedTranslationOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, scStateData) - unitTestSim.AddModelToTask(unitTaskName, prescribedStateData) - unitTestSim.AddModelToTask(unitTaskName, dataLog) + unitTestSim.AddModelToTask(unitTaskName, prescribedTransStateData) # Initialize the simulation unitTestSim.InitializeSimulation() @@ -471,10 +460,10 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI r_BN_N = scStateData.r_BN_N sigma_BN = scStateData.sigma_BN omega_BN_B = scStateData.omega_BN_B - r_FM_M = dataLog.r_FM_M - rPrime_FM_M = dataLog.rPrime_FM_M - rPrimePrime_FM_M = dataLog.rPrimePrime_FM_M - timespan = dataLog.times() + r_FM_M = prescribedTransStateData.r_FM_M + rPrime_FM_M = prescribedTransStateData.rPrime_FM_M + rPrimePrime_FM_M = prescribedTransStateData.rPrimePrime_FM_M + timespan = prescribedTransStateData.times() r_FM_M_Final = r_FM_M[-1, :] rPrime_FM_M_Final = rPrime_FM_M[-1, :] diff --git a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.cpp b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.cpp index be232fbb51..df359396dd 100644 --- a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.cpp +++ b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.cpp @@ -83,19 +83,26 @@ void PrescribedMotionStateEffector::Reset(uint64_t currentClock) */ void PrescribedMotionStateEffector::writeOutputStateMessages(uint64_t currentClock) { - // Write the prescribed motion output message if it is linked - if (this->prescribedMotionOutMsg.isLinked()) + + // Write the prescribed translational motion output message if it is linked + if (this->prescribedTranslationOutMsg.isLinked()) { - PrescribedMotionMsgPayload prescribedMotionBuffer = this->prescribedMotionOutMsg.zeroMsgPayload; - eigenVector3d2CArray(this->r_FM_M, prescribedMotionBuffer.r_FM_M); - eigenVector3d2CArray(this->rPrime_FM_M, prescribedMotionBuffer.rPrime_FM_M); - eigenVector3d2CArray(this->rPrimePrime_FM_M, prescribedMotionBuffer.rPrimePrime_FM_M); - eigenVector3d2CArray(this->omega_FM_F, prescribedMotionBuffer.omega_FM_F); - eigenVector3d2CArray(this->omegaPrime_FM_F, prescribedMotionBuffer.omegaPrime_FM_F); + PrescribedTranslationMsgPayload prescribedTranslationBuffer = this->prescribedTranslationOutMsg.zeroMsgPayload; + eigenVector3d2CArray(this->r_FM_M, prescribedTranslationBuffer.r_FM_M); + eigenVector3d2CArray(this->rPrime_FM_M, prescribedTranslationBuffer.rPrime_FM_M); + eigenVector3d2CArray(this->rPrimePrime_FM_M, prescribedTranslationBuffer.rPrimePrime_FM_M); + this->prescribedTranslationOutMsg.write(&prescribedTranslationBuffer, this->moduleID, currentClock); + } + // Write the prescribed rotational motion output message if it is linked + if (this->prescribedRotationOutMsg.isLinked()) + { + PrescribedRotationMsgPayload prescribedRotationBuffer = this->prescribedRotationOutMsg.zeroMsgPayload; + eigenVector3d2CArray(this->omega_FM_F, prescribedRotationBuffer.omega_FM_F); + eigenVector3d2CArray(this->omegaPrime_FM_F, prescribedRotationBuffer.omegaPrime_FM_F); Eigen::Vector3d sigma_FM_loc = eigenMRPd2Vector3d(this->sigma_FM); - eigenVector3d2CArray(sigma_FM_loc, prescribedMotionBuffer.sigma_FM); - this->prescribedMotionOutMsg.write(&prescribedMotionBuffer, this->moduleID, currentClock); + eigenVector3d2CArray(sigma_FM_loc, prescribedRotationBuffer.sigma_FM); + this->prescribedRotationOutMsg.write(&prescribedRotationBuffer, this->moduleID, currentClock); } // Write the config log message if it is linked @@ -318,22 +325,30 @@ void PrescribedMotionStateEffector::UpdateState(uint64_t currentSimNanos) // Store the current simulation time this->currentSimTimeSec = currentSimNanos * NANO2SEC; - // Read the input message if it is linked and written - if (this->prescribedMotionInMsg.isLinked() && this->prescribedMotionInMsg.isWritten()) + // Read the translational input message if it is linked and written + if (this->prescribedTranslationInMsg.isLinked() && this->prescribedTranslationInMsg.isWritten()) + { + PrescribedTranslationMsgPayload incomingPrescribedTransStates = this->prescribedTranslationInMsg(); + this->r_FM_M = cArray2EigenVector3d(incomingPrescribedTransStates.r_FM_M); + this->rPrime_FM_M = cArray2EigenVector3d(incomingPrescribedTransStates.rPrime_FM_M); + this->rPrimePrime_FM_M = cArray2EigenVector3d(incomingPrescribedTransStates.rPrimePrime_FM_M); + + // Save off the prescribed translational states at each dynamics time step + this->rEpoch_FM_M = cArray2EigenVector3d(incomingPrescribedTransStates.r_FM_M); + this->rPrimeEpoch_FM_M = cArray2EigenVector3d(incomingPrescribedTransStates.rPrime_FM_M); + } + + // Read the rotational input message if it is linked and written + if (this->prescribedRotationInMsg.isLinked() && this->prescribedRotationInMsg.isWritten()) { - PrescribedMotionMsgPayload incomingPrescribedStates = this->prescribedMotionInMsg(); - this->r_FM_M = cArray2EigenVector3d(incomingPrescribedStates.r_FM_M); - this->rPrime_FM_M = cArray2EigenVector3d(incomingPrescribedStates.rPrime_FM_M); - this->rPrimePrime_FM_M = cArray2EigenVector3d(incomingPrescribedStates.rPrimePrime_FM_M); - this->omega_FM_F = cArray2EigenVector3d(incomingPrescribedStates.omega_FM_F); - this->omegaPrime_FM_F = cArray2EigenVector3d(incomingPrescribedStates.omegaPrime_FM_F); - this->sigma_FM = cArray2EigenVector3d(incomingPrescribedStates.sigma_FM); - - // Save off the prescribed states at each dynamics time step - this->rEpoch_FM_M = cArray2EigenVector3d(incomingPrescribedStates.r_FM_M); - this->rPrimeEpoch_FM_M = cArray2EigenVector3d(incomingPrescribedStates.rPrime_FM_M); - this->omegaEpoch_FM_F = cArray2EigenVector3d(incomingPrescribedStates.omega_FM_F); - Eigen::Vector3d sigma_FM_loc = cArray2EigenVector3d(incomingPrescribedStates.sigma_FM); + PrescribedRotationMsgPayload incomingPrescribedRotStates = this->prescribedRotationInMsg(); + this->omega_FM_F = cArray2EigenVector3d(incomingPrescribedRotStates.omega_FM_F); + this->omegaPrime_FM_F = cArray2EigenVector3d(incomingPrescribedRotStates.omegaPrime_FM_F); + this->sigma_FM = cArray2EigenVector3d(incomingPrescribedRotStates.sigma_FM); + + // Save off the prescribed rotational states at each dynamics time step + this->omegaEpoch_FM_F = cArray2EigenVector3d(incomingPrescribedRotStates.omega_FM_F); + Eigen::Vector3d sigma_FM_loc = cArray2EigenVector3d(incomingPrescribedRotStates.sigma_FM); this->sigma_FMState->setState(sigma_FM_loc); } diff --git a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.h b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.h index 0bf5c54d24..034c2758a5 100644 --- a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.h +++ b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.h @@ -25,7 +25,8 @@ #include "simulation/dynamics/_GeneralModuleFiles/stateData.h" #include "architecture/messaging/messaging.h" #include "architecture/msgPayloadDefC/SCStatesMsgPayload.h" -#include "architecture/msgPayloadDefC/PrescribedMotionMsgPayload.h" +#include "architecture/msgPayloadDefC/PrescribedTranslationMsgPayload.h" +#include "architecture/msgPayloadDefC/PrescribedRotationMsgPayload.h" #include "architecture/utilities/avsEigenSupport.h" #include "architecture/utilities/avsEigenMRP.h" @@ -73,9 +74,11 @@ class PrescribedMotionStateEffector: public StateEffector, public SysModel { Eigen::MRPd sigma_FM; //!< MRP attitude of frame F relative to frame M std::string nameOfsigma_FMState; //!< Identifier for the sigma_FM state data container - ReadFunctor prescribedMotionInMsg; //!< Input message for the effector's prescribed states - Message prescribedMotionOutMsg; //!< Output message for the effector's prescribed states - Message prescribedMotionConfigLogOutMsg; //!< Output config log message for the effector's states + ReadFunctor prescribedTranslationInMsg; //!< Input message for the effector's translational prescribed states + ReadFunctor prescribedRotationInMsg; //!< Input message for the effector's rotational prescribed states + Message prescribedTranslationOutMsg; //!< Output message for the effector's translational prescribed states + Message prescribedRotationOutMsg; //!< Output message for the effector's rotational prescribed states + Message prescribedMotionConfigLogOutMsg; //!< Output config log message for the effector's states private: static uint64_t effectorID; //!< ID number of this panel diff --git a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.i b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.i index f247964841..59e8acf127 100644 --- a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.i +++ b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.i @@ -39,8 +39,10 @@ from Basilisk.architecture.swig_common_model import * %include "architecture/msgPayloadDefC/SCStatesMsgPayload.h" struct SCStatesMsg_C; -%include "architecture/msgPayloadDefC/PrescribedMotionMsgPayload.h" -struct PrescribedMotionMsg_C; +%include "architecture/msgPayloadDefC/PrescribedTranslationMsgPayload.h" +struct PrescribedTranslationMsg_C; +%include "architecture/msgPayloadDefC/PrescribedRotationMsgPayload.h" +struct PrescribedRotationMsg_C; %pythoncode %{ import sys diff --git a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.rst b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.rst index 2bbdd8c9b9..35146e70a6 100644 --- a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.rst +++ b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.rst @@ -4,15 +4,17 @@ Executive Summary The prescribed motion class is an instantiation of the state effector abstract class. This module describes the dynamics of a six-degree of freedom (6 DOF) prescribed rigid body connected to a central rigid spacecraft hub. The body frame for the prescribed body is designated by the frame :math:`\mathcal{F}`. The prescribed body is mounted onto a hub-fixed -interface described by a mount frame :math:`\mathcal{M}` that is fixed with respect to the hub. The prescribed body may -be commanded to translate and rotate in three-dimensional space with respect to the interface it is mounted on. -Accordingly, the prescribed states for the secondary body are written with respect to the mount frame, :math:`\mathcal{M}`. The -prescribed states are: ``r_FM_M``, ``rPrime_FM_M``, ``rPrimePrime_FM_M``, ``omega_FM_F``, ``omegaPrime_FM_F``, and -``sigma_FM``. - -The states of the prescribed body are not defined in this module. Therefore, a flight software profiler module must be -connected to this module's :ref:`PrescribedMotionMsgPayload` input message to profile the prescribed body's states as a -function of time. This message connection is required to provide the prescribed body's states to this dynamics module. +interface described by a mount frame :math:`\mathcal{M}`. The prescribed body may be commanded to translate and rotate +in three-dimensional space with respect to the interface it is mounted on. Accordingly, the prescribed states for +the secondary body are written with respect to the mount frame, :math:`\mathcal{M}`. The prescribed states are: +``r_FM_M``, ``rPrime_FM_M``, ``rPrimePrime_FM_M``, ``omega_FM_F``, ``omegaPrime_FM_F``, and ``sigma_FM``. + +The states of the prescribed body are not defined in this module. Therefore, separate kinematic profiler modules must +be connected to this module's :ref:`PrescribedTranslationMsgPayload` and :ref:`PrescribedRotationMsgPayload` +input messages to profile the prescribed body's states as a function of time. These message connections are required +to provide the prescribed body's states to this dynamics module. Note that either a single profiler can be connected to +these input messages or two separate profiler modules can be used; where one profiles the prescribed body's +translational states and the other profiles the prescribed body's rotational states. Message Connection Descriptions ------------------------------- @@ -28,12 +30,18 @@ provides information on what this message is used for. * - Msg Variable Name - Msg Type - Description - * - prescribedMotionInMsg - - :ref:`PrescribedMotionMsgPayload` - - Input message for the effector's prescribed states - * - prescribedMotionOutMsg - - :ref:`PrescribedMotionMsgPayload` - - Output message for the effector's prescribed states + * - prescribedTranslationInMsg + - :ref:`PrescribedTranslationMsgPayload` + - Input message for the effector's translational prescribed states + * - prescribedRotationInMsg + - :ref:`PrescribedRotationMsgPayload` + - Input message for the effector's rotational prescribed states + * - prescribedTranslationOutMsg + - :ref:`PrescribedTranslationMsgPayload` + - Output message for the effector's translational prescribed states + * - prescribedRotationOutMsg + - :ref:`PrescribedRotationMsgPayload` + - Output message for the effector's rotational prescribed states * - prescribedMotionConfigLogOutMsg - :ref:`SCStatesMsgPayload` - Output message containing the effector's inertial position and attitude states @@ -59,17 +67,17 @@ The rotational equations of motion are: Module Testing ^^^^^^^^^^^^^^ -The unit test for this module is an integrated test with two flight software profiler modules. This is required -because the dynamics module must be connected to a flight software profiler module to define the states of the +The unit test for this module is an integrated test with two kinematic profiler modules. This is required +because the dynamics module must be connected to kinematic profiler modules to define the states of the prescribed secondary body that is connected to the rigid spacecraft hub. The integrated test for this module has -two simple scenarios it is testing. The first scenario prescribes a 1 DOF rotational attitude maneuver for the -prescribed body using the :ref:`prescribedRot1DOF` flight software module. The second scenario prescribes a -translational maneuver for the prescribed body using the :ref:`prescribedTrans` flight software module. +two simple scenarios it is testing. The first scenario prescribes a 1 DOF rotation for the +prescribed body using the :ref:`prescribedRot1DOF` profiler module. The second scenario prescribes a 1 DOF +translation for the prescribed body using the :ref:`prescribedTrans` profiler module. -The unit test ensures that the profiled 1 DOF rotational attitude maneuver is properly computed for a series of +The 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. The unit test also ensures that the profiled translational maneuver is properly computed for a +``thetaDot_Ref``, respectively. The unit test also ensures that the profiled translation is properly computed for a series of initial and reference positions and maximum accelerations. The final prescribed position magnitude ``r_FM_M_Final`` and velocity magnitude ``rPrime_FM_M_Final`` are compared with the reference values ``r_FM_M_Ref`` and ``rPrime_FM_M_Ref``, respectively. Additionally for each scenario, the conservation quantities of orbital angular momentum, From 4b186f1f2869c2bcbdf01790ec3b34f772a0d709 Mon Sep 17 00:00:00 2001 From: Leah Kiner Date: Tue, 13 Feb 2024 11:53:14 -0700 Subject: [PATCH 7/7] Update release notes --- docs/source/Support/bskReleaseNotes.rst | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/source/Support/bskReleaseNotes.rst b/docs/source/Support/bskReleaseNotes.rst index 60a390c5e9..31608fbc95 100644 --- a/docs/source/Support/bskReleaseNotes.rst +++ b/docs/source/Support/bskReleaseNotes.rst @@ -46,6 +46,8 @@ Version |release| - Refactored the :ref:`PrescribedTransMsgPayload` message by renaming the message to :ref:`LinearTranslationRigidBodyMsgPayload` and renaming the message variables from ``scalarPos`` and ``scalarVel`` to ``rho`` and ``rhoDot`` +- Deprecated the :ref:`prescribedMotionMsgPayload` message and replaced with two separate + :ref:`prescribedTranslationMsgPayload` and :ref:`prescribedRotationMsgPayload` messages. Version 2.2.1 (Dec. 22, 2023) -----------------------------