diff --git a/docs/source/Support/bskReleaseNotes.rst b/docs/source/Support/bskReleaseNotes.rst index 1533ecfa28..60be020cfe 100644 --- a/docs/source/Support/bskReleaseNotes.rst +++ b/docs/source/Support/bskReleaseNotes.rst @@ -68,6 +68,12 @@ Version |release| - ``ConfigureStopTime`` now supports specifying the stop condition as ``<=`` (default, prior behavior) or ``>=`` (new). The new option is useful when the user wants to ensure that the simulation runs for at least the specified time, instead of at most the specified time. +- Configured the :ref:`prescribedMotionStateEffector` module for attachment of other state effectors to it + rather than the spacecraft hub. +- Configured the :ref:`spinningBodyOneDOFStateEffector`, :ref:`spinningBodyTwoDOFStateEffector`, and + :ref:`linearTranslationOneDOFStateEffector` modules for optional attachment to the prescribed motion state effector. +- Added two example branching scenarios to illustrate the prescribed motion branching capability. See + :ref:`scenarioPrescribedMotoinWithTranslationBranching` and :ref:`scenarioPrescribedMotoinWithRotationBranching`. Version 2.8.0 (August 30, 2025) diff --git a/docs/source/_images/static/scenarioPrescribedMotionWithRotationBranching.svg b/docs/source/_images/static/scenarioPrescribedMotionWithRotationBranching.svg new file mode 100644 index 0000000000..3a19c8862e --- /dev/null +++ b/docs/source/_images/static/scenarioPrescribedMotionWithRotationBranching.svg @@ -0,0 +1,748 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Canvas 1 + + Layer 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/docs/source/_images/static/scenarioPrescribedMotionWithTranslationBranching.svg b/docs/source/_images/static/scenarioPrescribedMotionWithTranslationBranching.svg new file mode 100644 index 0000000000..91e09af7b3 --- /dev/null +++ b/docs/source/_images/static/scenarioPrescribedMotionWithTranslationBranching.svg @@ -0,0 +1,710 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Canvas 1 + + + Layer 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Lander stowed configuration. + + + + + Lander deployed configuration. + + + + + diff --git a/examples/scenarioDeployingSolarArrays.py b/examples/scenarioDeployingSolarArrays.py index 9428077eb9..101dea5f91 100644 --- a/examples/scenarioDeployingSolarArrays.py +++ b/examples/scenarioDeployingSolarArrays.py @@ -252,36 +252,32 @@ def run(show_plots): array2ElementList.append(prescribedMotionStateEffector.PrescribedMotionStateEffector()) array1ElementList[i].ModelTag = "array1Element" + str(i + 1) array2ElementList[i].ModelTag = "array2Element" + str(i + 1) - array1ElementList[i].mass = mass_element # [kg] - array2ElementList[i].mass = mass_element # [kg] - array1ElementList[i].IPntPc_P = IElement_PntPc_P # [kg m^2] - array2ElementList[i].IPntPc_P = IElement_PntPc_P # [kg m^2] - array1ElementList[i].r_MB_B = r_M1B_B # [m] - array2ElementList[i].r_MB_B = r_M2B_B # [m] - array1ElementList[i].r_PcP_P = [- radius_array * np.cos(72 * macros.D2R), + array1ElementList[i].setMass(mass_element) # [kg] + array2ElementList[i].setMass(mass_element) # [kg] + array1ElementList[i].setIPntPc_P(IElement_PntPc_P) # [kg m^2] + array2ElementList[i].setIPntPc_P(IElement_PntPc_P) # [kg m^2] + array1ElementList[i].setR_MB_B(r_M1B_B) # [m] + array2ElementList[i].setR_MB_B(r_M2B_B) # [m] + array1ElementList[i].setR_PcP_P([- radius_array * np.cos(72 * macros.D2R), 0.0, - (1/3) * radius_array * np.sin(72 * macros.D2R)] # [m] For triangular wedge - array2ElementList[i].r_PcP_P = [radius_array * np.cos(72 * macros.D2R), + (1/3) * radius_array * np.sin(72 * macros.D2R)]) # [m] For triangular wedge + array2ElementList[i].setR_PcP_P([radius_array * np.cos(72 * macros.D2R), 0.0, - (1/3) * radius_array * np.sin(72 * macros.D2R)] # [m] For triangular wedge - array1ElementList[i].r_PM_M = r_PM1_M1Init1 # [m] - array2ElementList[i].r_PM_M = r_PM2_M2Init1 # [m] - array1ElementList[i].rPrime_PM_M = np.array([0.0, 0.0, 0.0]) # [m/s] - array2ElementList[i].rPrime_PM_M = np.array([0.0, 0.0, 0.0]) # [m/s] - array1ElementList[i].rPrimePrime_PM_M = np.array([0.0, 0.0, 0.0]) # [m/s^2] - array2ElementList[i].rPrimePrime_PM_M = np.array([0.0, 0.0, 0.0]) # [m/s^2] - array1ElementList[i].omega_PM_P = np.array([0.0, 0.0, 0.0]) # [rad/s] - array2ElementList[i].omega_PM_P = np.array([0.0, 0.0, 0.0]) # [rad/s] - array1ElementList[i].omegaPrime_PM_P = np.array([0.0, 0.0, 0.0]) # [rad/s^2] - array2ElementList[i].omegaPrime_PM_P = np.array([0.0, 0.0, 0.0]) # [rad/s^2] - array1ElementList[i].sigma_PM = sigma_PM1Init1 - array2ElementList[i].sigma_PM = sigma_PM2Init1 - array1ElementList[i].omega_MB_B = [0.0, 0.0, 0.0] # [rad/s] - array2ElementList[i].omega_MB_B = [0.0, 0.0, 0.0] # [rad/s] - array1ElementList[i].omegaPrime_MB_B = [0.0, 0.0, 0.0] # [rad/s^2] - array2ElementList[i].omegaPrime_MB_B = [0.0, 0.0, 0.0] # [rad/s^2] - array1ElementList[i].sigma_MB = [0.0, 0.0, 0.0] - array2ElementList[i].sigma_MB = [0.0, 0.0, 0.0] + (1/3) * radius_array * np.sin(72 * macros.D2R)]) # [m] For triangular wedge + array1ElementList[i].setR_PM_M(r_PM1_M1Init1) # [m] + array2ElementList[i].setR_PM_M(r_PM2_M2Init1) # [m] + array1ElementList[i].setRPrime_PM_M(np.array([0.0, 0.0, 0.0])) # [m/s] + array2ElementList[i].setRPrime_PM_M(np.array([0.0, 0.0, 0.0])) # [m/s] + array1ElementList[i].setRPrimePrime_PM_M(np.array([0.0, 0.0, 0.0])) # [m/s^2] + array2ElementList[i].setRPrimePrime_PM_M(np.array([0.0, 0.0, 0.0])) # [m/s^2] + array1ElementList[i].setOmega_PM_P(np.array([0.0, 0.0, 0.0])) # [rad/s] + array2ElementList[i].setOmega_PM_P(np.array([0.0, 0.0, 0.0])) # [rad/s] + array1ElementList[i].setOmegaPrime_PM_P(np.array([0.0, 0.0, 0.0])) # [rad/s^2] + array2ElementList[i].setOmegaPrime_PM_P(np.array([0.0, 0.0, 0.0])) # [rad/s^2] + array1ElementList[i].setSigma_PM(sigma_PM1Init1) + array2ElementList[i].setSigma_PM(sigma_PM2Init1) + array1ElementList[i].setSigma_MB([0.0, 0.0, 0.0]) + array2ElementList[i].setSigma_MB([0.0, 0.0, 0.0]) scObject.addStateEffector(array1ElementList[i]) scObject.addStateEffector(array2ElementList[i]) @@ -463,9 +459,9 @@ def run(show_plots): array1ElementRefMsgList2 = list() for i in range(num_elements): array1ElementList[i].prescribedTranslationInMsg.subscribeTo(array1ElementTranslationMessage) - array1ElementList[i].r_PcP_P = [0.0, 0.0, - (2/3) * radius_array * np.sin(72 * macros.D2R)] - array1ElementList[i].r_PM_M = r_PM1_M1Init2 # [m] - array1ElementList[i].sigma_PM = sigma_PM1Init2 + array1ElementList[i].setR_PcP_P([0.0, 0.0, - (2/3) * radius_array * np.sin(72 * macros.D2R)]) + array1ElementList[i].setR_PM_M(r_PM1_M1Init2) # [m] + array1ElementList[i].setSigma_PM(sigma_PM1Init2) array1RotProfilerList[i].setThetaInit(array1ThetaInit2) # [rad] array1RotProfilerList[i].setThetaDDotMax(array1MaxRotAccelList2[i]) # [rad/s^2] @@ -527,9 +523,9 @@ def run(show_plots): array2ElementRefMsgList3 = list() for i in range(num_elements): array2ElementList[i].prescribedTranslationInMsg.subscribeTo(array2ElementTranslationMessage) - array2ElementList[i].r_PcP_P = [0.0, 0.0, - (2/3) * radius_array * np.sin(72 * macros.D2R)] - array2ElementList[i].r_PM_M = r_PM2_M2Init2 # [m] - array2ElementList[i].sigma_PM = sigma_PM2Init2 + array2ElementList[i].setR_PcP_P([0.0, 0.0, - (2/3) * radius_array * np.sin(72 * macros.D2R)]) + array2ElementList[i].setR_PM_M(r_PM2_M2Init2) # [m] + array2ElementList[i].setSigma_PM(sigma_PM2Init2) array2RotProfilerList[i].setThetaInit(array2ThetaInit2) # [rad] array2RotProfilerList[i].setThetaDDotMax(array2MaxRotAccelList3[i]) # [rad/s^2] diff --git a/examples/scenarioPrescribedMotionWithRotationBranching.py b/examples/scenarioPrescribedMotionWithRotationBranching.py new file mode 100644 index 0000000000..1c1e9be9cd --- /dev/null +++ b/examples/scenarioPrescribedMotionWithRotationBranching.py @@ -0,0 +1,511 @@ +# ISC License +# +# Copyright (c) 2025, 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. + +r""" +Overview +-------- + +This scenario demonstrates the branching capability of the prescribed motion module using the +spinningBodyOneDOFStateEffector module. The prescribed motion module can be configured to have state effectors +attached to it rather than the spacecraft hub. To do so, the state effectors are set up normally, except their +parameters are set with respect to the prescribed motion effector rather than the hub. The parameters that were +previously required to be provided relative to the hub frame must instead be provided relative to the prescribed +motion frame. + +This example configures an ISS-scale spacecraft containing a cylindrical rigid hub, +two large prescribed trusses which rotate about their longitudinal axes, and eight single-axis solar panels +which rotate about their transverse (bending) axes. The spacecraft configuration is shown below. + +.. image:: /_images/static/scenarioPrescribedMotionWithRotationBranching.svg + :align: center + +The spacecraft is at rest at the start of the simulation. +To demonstrate the impact of the prescribed truss motion on the solar panel dynamics, all solar panels are +given zero initial deflections. The trusses are profiled to symmetrically rotate 45 degrees from their +initial configurations using a smoothed bang-coast-bang acceleration profile. + +The prescribed truss motion, solar panel deflections and their rates, and the hub response to the panel deflections +is plotted in this scenario. + +Illustration of Simulation Results +---------------------------------- + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithRotationBranching_1.svg + :align: center + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithRotationBranching_2.svg + :align: center + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithRotationBranching_3.svg + :align: center + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithRotationBranching_4.svg + :align: center + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithRotationBranching_5.svg + :align: center + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithRotationBranching_6.svg + :align: center + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithRotationBranching_7.svg + :align: center + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithRotationBranching_8.svg + :align: center + +""" + +# +# Prescribed Motion with 1-DOF Rotation Branching Scenario +# Author: Leah Kiner +# Creation Date: October 1, 2025 +# + +import inspect +import matplotlib.pyplot as plt +import numpy as np +import os + +from Basilisk.utilities import SimulationBaseClass, unitTestSupport, macros +from Basilisk.simulation import prescribedMotionStateEffector, spacecraft, spinningBodyOneDOFStateEffector +from Basilisk.simulation import prescribedLinearTranslation +from Basilisk.simulation import prescribedRotation1DOF +from Basilisk.architecture import messaging +from Basilisk.utilities import RigidBodyKinematics as rbk +from Basilisk.utilities import vizSupport + +filename = os.path.basename(os.path.splitext(__file__)[0]) +path = os.path.dirname(os.path.abspath(filename)) + +def run(show_plots): + + # Set up the simulation + sc_sim = SimulationBaseClass.SimBaseClass() + sim_process_name = "simProcess" + sim_process = sc_sim.CreateNewProcess(sim_process_name) + dyn_time_step_sec = 0.01 # [s] + fsw_time_step_sec = 0.1 # [s] + data_rec_time_step_sec = 0.1 # [s] + dyn_task_name = "dynTask" + data_rec_task_name = "dataRecTask" + sim_process.addTask(sc_sim.CreateNewTask(dyn_task_name, macros.sec2nano(dyn_time_step_sec))) + sim_process.addTask(sc_sim.CreateNewTask("fswTask", macros.sec2nano(fsw_time_step_sec))) + sim_process.addTask(sc_sim.CreateNewTask(data_rec_task_name, macros.sec2nano(data_rec_time_step_sec))) + + # Create the spacecraft hub + mass_hub = 15000 # [kg] + length_hub = 8 # [m] + width_hub = 8 # [m] + depth_hub = 20 # [m] + I_hub_11 = (1 / 12) * mass_hub * (length_hub * length_hub + depth_hub * depth_hub) # [kg m^2] + I_hub_22 = (1 / 12) * mass_hub * (length_hub * length_hub + width_hub * width_hub) # [kg m^2] + I_hub_33 = (1 / 12) * mass_hub * (width_hub * width_hub + depth_hub * depth_hub) # [kg m^2] + + sc_object = spacecraft.Spacecraft() + sc_object.ModelTag = "scObject" + sc_object.hub.mHub = mass_hub # kg + sc_object.hub.r_BcB_B = [0.0, 0.0, 0.0] # [m] + sc_object.hub.IHubPntBc_B = [[I_hub_11, 0.0, 0.0], [0.0, I_hub_22, 0.0], [0.0, 0.0, I_hub_33]] # [kg m^2] + sc_object.hub.r_CN_NInit = [[0.0], [0.0], [0.0]] + sc_object.hub.v_CN_NInit = [[0.0], [0.0], [0.0]] + sc_object.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] + sc_object.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] + sc_sim.AddModelToTask(dyn_task_name, sc_object) + + # Shared prescribed array boom parameters + prescribed_pos_init = 0.0 + prescribed_theta_init = 0.0 + prescribed_trans_axis_M = np.array([1.0, 0.0, 0.0]) + prescribed_rot_axis_M = np.array([1.0, 0.0, 0.0]) + prv_P0M = prescribed_theta_init * prescribed_rot_axis_M + sigma_P0M = rbk.PRV2MRP(prv_P0M) + mass_prescribed = 6000.0 # [kg] + length_prescribed = 50.0 # [m] + width_prescribed = 4.0 # [m] + depth_prescribed = 4.0 # [m] + I_prescribed_11 = (1 / 12) * mass_prescribed * (length_prescribed * length_prescribed + depth_prescribed * depth_prescribed) # [kg m^2] + I_prescribed_22 = (1 / 12) * mass_prescribed * (length_prescribed * length_prescribed + width_prescribed * width_prescribed) # [kg m^2] + I_prescribed_33 = (1 / 12) * mass_prescribed * (width_prescribed * width_prescribed + depth_prescribed * depth_prescribed) # [kg m^2] + I_prescribed_Pc_P = [[I_prescribed_11, 0.0, 0.0], [0.0, I_prescribed_22, 0.0], [0.0, 0.0,I_prescribed_33]] # [kg m^2] + + # Create prescribed array boom 1 + prescribed_truss_1 = prescribedMotionStateEffector.PrescribedMotionStateEffector() + prescribed_truss_1.ModelTag = "prescribedTruss1" + prescribed_truss_1.setMass(mass_prescribed) + prescribed_truss_1.setIPntPc_P(I_prescribed_Pc_P) + prescribed_truss_1.setR_MB_B([4.0, 0.0, 0.0]) + prescribed_truss_1.setR_PcP_P([25.0, 0.0, 0.0]) + prescribed_truss_1.setR_PM_M([0.0, 0.0, 0.0]) + prescribed_truss_1.setRPrime_PM_M(np.array([0.0, 0.0, 0.0])) + prescribed_truss_1.setRPrimePrime_PM_M(np.array([0.0, 0.0, 0.0])) + prescribed_truss_1.setOmega_PM_P(np.array([0.0, 0.0, 0.0])) + prescribed_truss_1.setOmegaPrime_PM_P(np.array([0.0, 0.0, 0.0])) + prescribed_truss_1.setSigma_PM(sigma_P0M) + prescribed_truss_1.setSigma_MB([0.0, 0.0, 0.0]) + sc_sim.AddModelToTask(dyn_task_name, prescribed_truss_1) + sc_object.addStateEffector(prescribed_truss_1) + + # Create prescribed array boom 2 + prescribed_truss_2 = prescribedMotionStateEffector.PrescribedMotionStateEffector() + prescribed_truss_2.ModelTag = "prescribedTruss2" + prescribed_truss_2.setMass(mass_prescribed) + prescribed_truss_2.setIPntPc_P(I_prescribed_Pc_P) + prescribed_truss_2.setR_MB_B([-4.0, 0.0, 0.0]) + prescribed_truss_2.setR_PcP_P([25.0, 0.0, 0.0]) + prescribed_truss_2.setR_PM_M([0.0, 0.0, 0.0]) + prescribed_truss_2.setRPrime_PM_M(np.array([0.0, 0.0, 0.0])) + prescribed_truss_2.setRPrimePrime_PM_M(np.array([0.0, 0.0, 0.0])) + prescribed_truss_2.setOmega_PM_P(np.array([0.0, 0.0, 0.0])) + prescribed_truss_2.setOmegaPrime_PM_P(np.array([0.0, 0.0, 0.0])) + prescribed_truss_2.setSigma_PM(sigma_P0M) + dcm_MB = np.array([[-1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, -1.0]]) + prescribed_truss_2.setSigma_MB(rbk.C2MRP(dcm_MB)) + sc_sim.AddModelToTask(dyn_task_name, prescribed_truss_2) + sc_object.addStateEffector(prescribed_truss_2) + + # Shared prescribed motion profiler parameters + prescribed_ang_accel_max = 0.5 * macros.D2R # [rad/s^2] + prescribed_trans_accel_max = 0.005 # [m/s^2] + prescribed_theta_ref = 45.0 * macros.D2R # [rad] + prescribed_pos_ref = 0.0 # [m] + + # Create prescribed array boom 1 rotational motion profiler + one_dof_rotation_profiler_1 = prescribedRotation1DOF.PrescribedRotation1DOF() + one_dof_rotation_profiler_1.ModelTag = "prescribedRotation1DOF_1" + one_dof_rotation_profiler_1.setRotHat_M(prescribed_rot_axis_M) + one_dof_rotation_profiler_1.setThetaDDotMax(prescribed_ang_accel_max) + one_dof_rotation_profiler_1.setThetaInit(prescribed_theta_init) + one_dof_rotation_profiler_1.setCoastOptionBangDuration(1.0) + one_dof_rotation_profiler_1.setSmoothingDuration(1.0) + sc_sim.AddModelToTask(dyn_task_name, one_dof_rotation_profiler_1) + + # Create prescribed array boom 2 rotational motion profiler + one_dof_rotation_profiler_2 = prescribedRotation1DOF.PrescribedRotation1DOF() + one_dof_rotation_profiler_2.ModelTag = "prescribedRotation1DOF_2" + one_dof_rotation_profiler_2.setRotHat_M(prescribed_rot_axis_M) + one_dof_rotation_profiler_2.setThetaDDotMax(prescribed_ang_accel_max) + one_dof_rotation_profiler_2.setThetaInit(prescribed_theta_init) + one_dof_rotation_profiler_2.setCoastOptionBangDuration(1.0) + one_dof_rotation_profiler_2.setSmoothingDuration(1.0) + sc_sim.AddModelToTask(dyn_task_name, one_dof_rotation_profiler_2) + + # Create prescribed array boom 1 rotational motion reference message + prescribed_rotation_1_msg_data = messaging.HingedRigidBodyMsgPayload() + prescribed_rotation_1_msg_data.theta = prescribed_theta_ref + prescribed_rotation_1_msg_data.thetaDot = 0.0 + prescribed_rotation_1_msg = messaging.HingedRigidBodyMsg().write(prescribed_rotation_1_msg_data) + one_dof_rotation_profiler_1.spinningBodyInMsg.subscribeTo(prescribed_rotation_1_msg) + prescribed_truss_1.prescribedRotationInMsg.subscribeTo(one_dof_rotation_profiler_1.prescribedRotationOutMsg) + + # Create prescribed array boom 2 rotational motion reference message + prescribed_rotation_2_msg_data = messaging.HingedRigidBodyMsgPayload() + prescribed_rotation_2_msg_data.theta = - prescribed_theta_ref + prescribed_rotation_2_msg_data.thetaDot = 0.0 + prescribed_rotation_2_msg = messaging.HingedRigidBodyMsg().write(prescribed_rotation_2_msg_data) + one_dof_rotation_profiler_2.spinningBodyInMsg.subscribeTo(prescribed_rotation_2_msg) + prescribed_truss_2.prescribedRotationInMsg.subscribeTo(one_dof_rotation_profiler_2.prescribedRotationOutMsg) + + # Create prescribed array boom 1 translational motion profiler + one_dof_translation_profiler_1 = prescribedLinearTranslation.PrescribedLinearTranslation() + one_dof_translation_profiler_1.ModelTag = "prescribedLinearTranslation_1" + one_dof_translation_profiler_1.setTransHat_M(prescribed_trans_axis_M) + one_dof_translation_profiler_1.setTransAccelMax(prescribed_trans_accel_max) + one_dof_translation_profiler_1.setTransPosInit(prescribed_pos_init) + one_dof_translation_profiler_1.setCoastOptionBangDuration(1.0) + one_dof_translation_profiler_1.setSmoothingDuration(1.0) + sc_sim.AddModelToTask(dyn_task_name, one_dof_translation_profiler_1) + + # Create prescribed array boom 2 translational motion profiler + one_dof_translation_profiler_2 = prescribedLinearTranslation.PrescribedLinearTranslation() + one_dof_translation_profiler_2.ModelTag = "prescribedLinearTranslation_2" + one_dof_translation_profiler_2.setTransHat_M(prescribed_trans_axis_M) + one_dof_translation_profiler_2.setTransAccelMax(prescribed_trans_accel_max) + one_dof_translation_profiler_2.setTransPosInit(prescribed_pos_init) + one_dof_translation_profiler_2.setCoastOptionBangDuration(1.0) + one_dof_translation_profiler_2.setSmoothingDuration(1.0) + sc_sim.AddModelToTask(dyn_task_name, one_dof_translation_profiler_2) + + # Create prescribed array boom translational motion reference message + prescribed_translation_msg_data = messaging.LinearTranslationRigidBodyMsgPayload() + prescribed_translation_msg_data.rho = prescribed_pos_ref + prescribed_translation_msg_data.rhoDot = 0.0 + prescribed_translation_msg = messaging.LinearTranslationRigidBodyMsg().write(prescribed_translation_msg_data) + one_dof_translation_profiler_1.linearTranslationRigidBodyInMsg.subscribeTo(prescribed_translation_msg) + one_dof_translation_profiler_2.linearTranslationRigidBodyInMsg.subscribeTo(prescribed_translation_msg) + prescribed_truss_1.prescribedTranslationInMsg.subscribeTo(one_dof_translation_profiler_1.prescribedTranslationOutMsg) + prescribed_truss_2.prescribedTranslationInMsg.subscribeTo(one_dof_translation_profiler_2.prescribedTranslationOutMsg) + + # Shared spinning body parameters + mass_panel = 1000.0 # [kg] + length_panel = 10.0 # [m] + width_panel = 0.3 # [m] + depth_panel = 30.0 # [m] + I_panel_11 = (1 / 12) * mass_panel * (length_panel * length_panel + depth_panel * depth_panel) # [kg m^2] + I_panel_22 = (1 / 12) * mass_panel * (length_panel * length_panel + width_panel * width_panel) # [kg m^2] + I_panel_33 = (1 / 12) * mass_panel * (width_prescribed * width_prescribed + depth_panel * depth_panel) # [kg m^2] + I_panel_ScS = [[I_panel_11, 0.0, 0.0], [0.0, I_panel_22, 0.0], [0.0, 0.0, I_panel_33]] # [kg m^2] + r_ScS_S = [[0.0], [0.0], [15.0]] + panel_s_hat_S = [[1], [0], [0]] + k = 700000.0 + c = 50000.0 + + r_S1B_B = [[50.0], [0.0], [2.0]] # [m] r_S1P_P + r_S2B_B = [[35.0], [0.0], [2.0]] # [m] r_S2P_P + r_S3B_B = [[50.0], [0.0], [-2.0]] # [m] r_S3P_P + r_S4B_B = [[35.0], [0.0], [-2.0]] # [m] r_S4P_P + r_S5B_B = [[35.0], [0.0], [-2.0]] # [m] r_S5P_P + r_S6B_B = [[50.0], [0.0], [-2.0]] # [m] r_S6P_P + r_S7B_B = [[35.0], [0.0], [2.0]] # [m] r_S7P_P + r_S8B_B = [[50.0], [0.0], [2.0]] # [m] r_S8P_P + + # Create the spinning bodies + num_panels = 8 + spinning_panel_list = list() + for idx in range(num_panels): + panel_num = idx + 1 + spinning_panel_list.append(spinningBodyOneDOFStateEffector.SpinningBodyOneDOFStateEffector()) + spinning_panel_list[idx].ModelTag = "spinningBody" + str(panel_num) + spinning_panel_list[idx].mass = mass_panel + spinning_panel_list[idx].IPntSc_S = I_panel_ScS + spinning_panel_list[idx].r_ScS_S = r_ScS_S + spinning_panel_list[idx].sHat_S = panel_s_hat_S + spinning_panel_list[idx].k = k + spinning_panel_list[idx].c = c + + if panel_num == 1 or panel_num == 2 or panel_num == 7 or panel_num == 8: + spinning_panel_list[idx].dcm_S0B = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] + else: + spinning_panel_list[idx].dcm_S0B = [[-1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, -1.0]] + + sc_sim.AddModelToTask(dyn_task_name, spinning_panel_list[idx]) + + spinning_panel_list[0].r_SB_B = r_S1B_B + spinning_panel_list[1].r_SB_B = r_S2B_B + spinning_panel_list[2].r_SB_B = r_S3B_B + spinning_panel_list[3].r_SB_B = r_S4B_B + spinning_panel_list[4].r_SB_B = r_S5B_B + spinning_panel_list[5].r_SB_B = r_S6B_B + spinning_panel_list[6].r_SB_B = r_S7B_B + spinning_panel_list[7].r_SB_B = r_S8B_B + + # Connect the solar panels to the correct prescribed truss + prescribed_truss_1.addStateEffector(spinning_panel_list[0]) + prescribed_truss_1.addStateEffector(spinning_panel_list[1]) + prescribed_truss_1.addStateEffector(spinning_panel_list[2]) + prescribed_truss_1.addStateEffector(spinning_panel_list[3]) + prescribed_truss_2.addStateEffector(spinning_panel_list[4]) + prescribed_truss_2.addStateEffector(spinning_panel_list[5]) + prescribed_truss_2.addStateEffector(spinning_panel_list[6]) + prescribed_truss_2.addStateEffector(spinning_panel_list[7]) + + # Set up data logging + sc_state_data_log = sc_object.scStateOutMsg.recorder() + one_dof_rotation_profiler_1_data_log = one_dof_rotation_profiler_1.spinningBodyOutMsg.recorder() + one_dof_rotation_profiler_2_data_log = one_dof_rotation_profiler_2.spinningBodyOutMsg.recorder() + prescribed_rotation_1_data_log = prescribed_truss_1.prescribedRotationOutMsg.recorder() + prescribed_rotation_2_data_log = prescribed_truss_2.prescribedRotationOutMsg.recorder() + spinning_panel_1_data_log = spinning_panel_list[0].spinningBodyOutMsg.recorder() + spinning_panel_3_data_log = spinning_panel_list[2].spinningBodyOutMsg.recorder() + sc_sim.AddModelToTask(data_rec_task_name, sc_state_data_log) + sc_sim.AddModelToTask(data_rec_task_name, one_dof_rotation_profiler_1_data_log) + sc_sim.AddModelToTask(data_rec_task_name, one_dof_rotation_profiler_2_data_log) + sc_sim.AddModelToTask(data_rec_task_name, prescribed_rotation_1_data_log) + sc_sim.AddModelToTask(data_rec_task_name, prescribed_rotation_2_data_log) + sc_sim.AddModelToTask(data_rec_task_name, spinning_panel_1_data_log) + sc_sim.AddModelToTask(data_rec_task_name, spinning_panel_3_data_log) + + # Add Vizard + sc_body_list = [sc_object] + sc_body_list.append(["prescribedTruss1", prescribed_truss_1.prescribedMotionConfigLogOutMsg]) + sc_body_list.append(["prescribedTruss2", prescribed_truss_2.prescribedMotionConfigLogOutMsg]) + for idx in range(num_panels): + sc_body_list.append(["spinningBody" + str(idx + 1), spinning_panel_list[idx].spinningBodyConfigLogOutMsg]) + + if vizSupport.vizFound: + viz = vizSupport.enableUnityVisualization(sc_sim, data_rec_task_name, sc_body_list, + saveFile=filename + ) + vizSupport.createCustomModel(viz + , simBodiesToModify=[sc_object.ModelTag] + , modelPath="CUBE" + , scale=[width_hub, length_hub, depth_hub] + , color=vizSupport.toRGBA255("gray")) + vizSupport.createCustomModel(viz + , simBodiesToModify=["prescribedTruss1"] + , modelPath="CUBE" + , scale=[length_prescribed, width_prescribed, depth_prescribed] + , color=vizSupport.toRGBA255("green")) + vizSupport.createCustomModel(viz + , simBodiesToModify=["prescribedTruss2"] + , modelPath="CUBE" + , scale=[length_prescribed, width_prescribed, depth_prescribed] + , color=vizSupport.toRGBA255("green")) + for idx in range(num_panels): + vizSupport.createCustomModel(viz + , simBodiesToModify=["spinningBody" + str(idx + 1)] + , modelPath="CUBE" + , scale=[length_panel, width_panel, depth_panel] + , color=vizSupport.toRGBA255("blue")) + viz.settings.orbitLinesOn = -1 + + # Run the simulation + sc_sim.InitializeSimulation() + sim_time = 90.0 # [s] + sc_sim.ConfigureStopTime(macros.sec2nano(sim_time)) + sc_sim.ExecuteSimulation() + + # Extract the logged variables + timespan = sc_state_data_log.times() * macros.NANO2SEC # [s] + r_BN_N = sc_state_data_log.r_BN_N # [m] + sigma_BN = sc_state_data_log.sigma_BN + omega_BN_B = sc_state_data_log.omega_BN_B * macros.R2D # [deg/s] + prescribed_theta_1 = one_dof_rotation_profiler_1_data_log.theta * macros.R2D # [deg] + prescribed_theta_2 = one_dof_rotation_profiler_2_data_log.theta * macros.R2D # [deg] + omega_P1M_P1 = prescribed_rotation_1_data_log.omega_PM_P * macros.R2D # [deg/s] + omega_P2M_P2 = prescribed_rotation_2_data_log.omega_PM_P * macros.R2D # [deg/s] + omega_prime_P1M_P1 = prescribed_rotation_1_data_log.omegaPrime_PM_P * macros.R2D # [deg/s^2] + omega_prime_P2M_P2 = prescribed_rotation_2_data_log.omegaPrime_PM_P * macros.R2D # [deg/s^2] + spinning_panel_1_theta = spinning_panel_1_data_log.theta * macros.R2D # [deg] + spinning_panel_3_theta = spinning_panel_3_data_log.theta * macros.R2D # [deg] + spinning_panel_1_theta_dot = spinning_panel_1_data_log.thetaDot * macros.R2D # [deg] + spinning_panel_3_theta_dot = spinning_panel_3_data_log.thetaDot * macros.R2D # [deg] + + figure_list = {} + plt.close("all") + + # Plot prescribed truss angles + plt.figure(1) + plt.clf() + plt.plot(timespan, prescribed_theta_1, label=r'$\theta_{\text{P}_1}$', color="teal") + plt.plot(timespan, prescribed_theta_2, label=r'$\theta_{\text{P}_2}$', color="darkviolet") + plt.title(r'Prescribed Truss Angles', fontsize=16) + plt.ylabel('(deg)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc="center right", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_1" + figure_list[plt_name] = plt.figure(1) + + # Plot prescribed truss angle rates + prescribed_theta_dot_1 = np.dot(omega_P1M_P1, prescribed_rot_axis_M) # [deg/s] + prescribed_theta_dot_2 = np.dot(omega_P2M_P2, prescribed_rot_axis_M) # [deg/s] + + plt.figure(2) + plt.clf() + plt.plot(timespan, prescribed_theta_dot_1, label=r'$\dot{\theta}_{\text{P}_1}$', color="teal") + plt.plot(timespan, prescribed_theta_dot_2, label=r'$\dot{\theta}_{\text{P}_2}$', color="darkviolet") + plt.title(r'Prescribed Truss Angle Rates', fontsize=16) + plt.ylabel('(deg/s)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc="upper right", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_2" + figure_list[plt_name] = plt.figure(2) + + # Plot prescribed truss accelerations + prescribed_theta_ddot_1 = np.dot(omega_prime_P1M_P1, prescribed_rot_axis_M) # [deg/s^2] + prescribed_theta_ddot_2 = np.dot(omega_prime_P2M_P2, prescribed_rot_axis_M) # [deg/s^2] + + plt.figure(3) + plt.clf() + plt.plot(timespan, prescribed_theta_ddot_1, label=r'$\ddot{\theta}_{\text{P}_1}$', color="teal") + plt.plot(timespan, prescribed_theta_ddot_2, label=r'$\ddot{\theta}_{\text{P}_2}$', color="darkviolet") + plt.title(r'Prescribed Truss Accelerations', fontsize=16) + plt.ylabel('(deg/s$^2$)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc="upper right", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_3" + figure_list[plt_name] = plt.figure(3) + + # Plot spinning body solar panel angles + plt.figure(4) + plt.clf() + plt.plot(timespan, spinning_panel_1_theta, label=r'$\theta_{\text{S}_{1,2,5,6}}$', color="teal") + plt.plot(timespan, spinning_panel_3_theta, label=r'$\theta_{\text{S}_{3,4,7,8}}$', color="darkviolet") + plt.title(r'Solar Array Angles', fontsize=16) + plt.ylabel('(deg)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc="upper right", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_4" + figure_list[plt_name] = plt.figure(4) + + # Plot spinning body solar panel angle rates + plt.figure(5) + plt.clf() + plt.plot(timespan, spinning_panel_1_theta_dot, label=r'$\dot{\theta}_{\text{S}_{1,2,5,6}}$', color="teal") + plt.plot(timespan, spinning_panel_3_theta_dot, label=r'$\dot{\theta}_{\text{S}_{3,4,7,8}}$', color="darkviolet") + plt.title(r'Solar Array Angle Rates', fontsize=16) + plt.ylabel('(deg/s)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc="upper right", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_5" + figure_list[plt_name] = plt.figure(5) + + # Plot hub inertial position + plt.figure(6) + plt.clf() + plt.plot(timespan, r_BN_N[:, 0], label=r'$r_1$', color="teal") + plt.plot(timespan, r_BN_N[:, 1], label=r'$r_2$', color="darkviolet") + plt.plot(timespan, r_BN_N[:, 2], label=r'$r_3$', color="blue") + plt.title(r'Hub Inertial Position ${}^\mathcal{N} r_{B/N}$', fontsize=16) + plt.ylabel('(m)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc="center left", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_6" + figure_list[plt_name] = plt.figure(6) + + # Plot hub inertial attitude + plt.figure(7) + plt.clf() + plt.plot(timespan, sigma_BN[:, 0], label=r'$\sigma_1$', color="teal") + plt.plot(timespan, sigma_BN[:, 1], label=r'$\sigma_2$', color="darkviolet") + plt.plot(timespan, sigma_BN[:, 2], label=r'$\sigma_3$', color="blue") + plt.title(r'Hub Inertial Attitude $\sigma_{\mathcal{B}/\mathcal{N}}$', fontsize=16) + plt.ylabel('', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc="center right", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_7" + figure_list[plt_name] = plt.figure(7) + + # Plot hub inertial angular velocity + plt.figure(8) + plt.clf() + plt.plot(timespan, omega_BN_B[:, 0], label=r'$\omega_1$', color="teal") + plt.plot(timespan, omega_BN_B[:, 1], label=r'$\omega_2$', color="darkviolet") + plt.plot(timespan, omega_BN_B[:, 2], label=r'$\omega_3$', color="blue") + plt.title(r'Hub Inertial Angular Velocity ${}^\mathcal{B} \omega_{\mathcal{B}/\mathcal{N}}$', fontsize=16) + plt.ylabel('(deg/s)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc="center right", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_8" + figure_list[plt_name] = plt.figure(8) + + if show_plots: + plt.show() + plt.close("all") + + return figure_list + + +if __name__ == "__main__": + run(True) diff --git a/examples/scenarioPrescribedMotionWithTranslationBranching.py b/examples/scenarioPrescribedMotionWithTranslationBranching.py new file mode 100644 index 0000000000..3f9460a2a1 --- /dev/null +++ b/examples/scenarioPrescribedMotionWithTranslationBranching.py @@ -0,0 +1,550 @@ +# ISC License +# +# Copyright (c) 2025, 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. + +r""" +Overview +-------- + +This scenario demonstrates the branching capability of the prescribed motion module using the +linearTranslationOneDOFStateEffector module. The prescribed motion module can be configured to have state effectors +attached to it rather than the spacecraft hub. To do so, the state effectors are set up normally, except their +parameters are set with respect to the prescribed motion effector rather than the hub. The parameters that were +previously required to be provided relative to the hub frame must instead be provided relative to the prescribed +motion frame. + +This example configures a spacecraft lander concept, where the spacecraft system is comprised of +a small cylindrical hub, a prescribed motion platform attached underneath the hub, and three symmetrically +canted telescoping landing struts that are used as shock absorbers to assist the spacecraft in safely landing. +The landing struts are each canted by 60 degrees relative to the platform base (30 degrees relative to the +platform normal), and are evenly spaced by 120 degrees surrounding the center of the platform base. The stowed and +deployed lander configurations are shown below. + +.. image:: /_images/static/scenarioPrescribedMotionWithTranslationBranching.svg + :align: center + +The spacecraft is at rest at the start of the simulation. The platform is actuated first, where it is commanded +to simultaneously translate and rotate downwards and away from primary hub structure to model +the initial phase of the lander deployment. Its reference translational and rotational displacements are chosen +arbitrarily as 10 degrees and 0.5 meters, respectively. Two smoothed bang-coast-bang acceleration profiles +are used to kinematically prescribe the required two-degree-of-freedom platform motion. After the platform finishes +actuating, the three landing struts are each commanded to deploy away from the platform along their translational +axes. Similarly, the struts are given arbitrary reference displacements of 0.2, 0.1, and 0.15 meters relative +to the platform. + +The prescribed platform motion, landing strut displacements and their rates, and the hub response to the lander +deployment is plotted in this scenario. + +Illustration of Simulation Results +---------------------------------- + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithTranslationBranching_1.svg + :align: center + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithTranslationBranching_2.svg + :align: center + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithTranslationBranching_3.svg + :align: center + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithTranslationBranching_4.svg + :align: center + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithTranslationBranching_5.svg + :align: center + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithTranslationBranching_6.svg + :align: center + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithTranslationBranching_7.svg + :align: center + +.. image:: /_images/Scenarios/scenarioPrescribedMotionWithTranslationBranching_8.svg + :align: center + +""" + +# +# Prescribed Motion with 1-DOF Linear Translation Branching Scenario +# Author: Leah Kiner +# Creation Date: October 1, 2025 +# + +import inspect +import matplotlib.pyplot as plt +import numpy as np +import os + +from Basilisk.utilities import SimulationBaseClass, unitTestSupport, macros +from Basilisk.simulation import prescribedMotionStateEffector, spacecraft, linearTranslationOneDOFStateEffector +from Basilisk.simulation import prescribedLinearTranslation +from Basilisk.simulation import prescribedRotation1DOF +from Basilisk.architecture import messaging +from Basilisk.utilities import RigidBodyKinematics as rbk +from Basilisk.utilities import vizSupport + +filename = os.path.basename(os.path.splitext(__file__)[0]) +path = os.path.dirname(os.path.abspath(filename)) + +def run(show_plots): + + # Set up the simulation + sc_sim = SimulationBaseClass.SimBaseClass() + sim_process_name = "simProcess" + sim_process = sc_sim.CreateNewProcess(sim_process_name) + dyn_time_step_sec = 0.01 # [s] + fsw_time_step_sec = 0.1 # [s] + data_rec_time_step_sec = 0.1 # [s] + dyn_task_name = "dynTask" + data_rec_task_name = "dataRecTask" + sim_process.addTask(sc_sim.CreateNewTask(dyn_task_name, macros.sec2nano(dyn_time_step_sec))) + sim_process.addTask(sc_sim.CreateNewTask("fswTask", macros.sec2nano(fsw_time_step_sec))) + sim_process.addTask(sc_sim.CreateNewTask(data_rec_task_name, macros.sec2nano(data_rec_time_step_sec))) + + # Create the spacecraft hub + mass_hub = 1000.0 # [kg] + length_hub = 1.0 # [m] + width_hub = 1.0 # [m] + depth_hub = 1.0 # [m] + I_hub_11 = (1 / 12) * mass_hub * (length_hub * length_hub + depth_hub * depth_hub) # [kg m^2] + I_hub_22 = (1 / 12) * mass_hub * (length_hub * length_hub + width_hub * width_hub) # [kg m^2] + I_hub_33 = (1 / 12) * mass_hub * (width_hub * width_hub + depth_hub * depth_hub) # [kg m^2] + + sc_object = spacecraft.Spacecraft() + sc_object.ModelTag = "scObject" + sc_object.hub.mHub = mass_hub # kg + sc_object.hub.r_BcB_B = [0.0, 0.0, 0.0] # [m] + sc_object.hub.IHubPntBc_B = [[I_hub_11, 0.0, 0.0], [0.0, I_hub_22, 0.0], [0.0, 0.0, I_hub_33]] # [kg m^2] (Hub approximated as a cube) + sc_object.hub.r_CN_NInit = [[0.0], [0.0], [0.0]] + sc_object.hub.v_CN_NInit = [[0.0], [0.0], [0.0]] + sc_object.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] + sc_object.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] + sc_sim.AddModelToTask(dyn_task_name, sc_object) + + # Prescribed platform parameters + mass_prescribed = 200 # [kg] + length_prescribed = 1.0 # [m] + width_prescribed = 1.0 # [m] + depth_prescribed = 0.2 # [m] + I_prescribed_11 = (1 / 12) * mass_prescribed * (length_prescribed * length_prescribed + depth_prescribed * depth_prescribed) # [kg m^2] + I_prescribed_22 = (1 / 12) * mass_prescribed * (length_prescribed * length_prescribed + width_prescribed * width_prescribed) # [kg m^2] + I_prescribed_33 = (1 / 12) * mass_prescribed * (width_prescribed * width_prescribed + depth_prescribed * depth_prescribed) # [kg m^2] + I_prescribed_Pc_P = [[I_prescribed_11, 0.0, 0.0], [0.0, I_prescribed_22, 0.0], [0.0, 0.0,I_prescribed_33]] # [kg m^2] + prescribed_theta_init = 0.0 + prescribed_trans_axis_M = np.array([0.0, 0.0, -1.0]) + prescribed_rot_axis_M = np.array([0.0, 1.0, 0.0]) + prv_P0M = prescribed_theta_init * prescribed_rot_axis_M + sigma_P0M = rbk.PRV2MRP(prv_P0M) + + # Create the prescribed platform + prescribed_platform = prescribedMotionStateEffector.PrescribedMotionStateEffector() + prescribed_platform.ModelTag = "prescribedPlatform" + prescribed_platform.setMass(mass_prescribed) + prescribed_platform.setIPntPc_P(I_prescribed_Pc_P) + prescribed_platform.setR_MB_B([0.0, 0.0, -0.5 * depth_hub]) + prescribed_platform.setR_PcP_P([0.0, 0.0, 0.5 * depth_prescribed]) + prescribed_platform.setR_PM_M([0.0, 0.0, -depth_prescribed]) + prescribed_platform.setRPrime_PM_M(np.array([0.0, 0.0, 0.0])) + prescribed_platform.setRPrimePrime_PM_M(np.array([0.0, 0.0, 0.0])) + prescribed_platform.setOmega_PM_P(np.array([0.0, 0.0, 0.0])) + prescribed_platform.setOmegaPrime_PM_P(np.array([0.0, 0.0, 0.0])) + prescribed_platform.setSigma_PM(sigma_P0M) + prescribed_platform.setSigma_MB([0.0, 0.0, 0.0]) + sc_sim.AddModelToTask(dyn_task_name, prescribed_platform) + sc_object.addStateEffector(prescribed_platform) + + # Shared prescribed motion profiler parameters + prescribed_ang_accel_max = 0.5 * macros.D2R # [rad/s^2] + prescribed_trans_accel_max = 0.0025 # [m/s^2] + prescribed_theta_ref = 10.0 * macros.D2R # [rad] + prescribed_pos_ref = 0.5 # [m] + + # Create prescribed platform rotational motion profiler + one_dof_rotation_profiler = prescribedRotation1DOF.PrescribedRotation1DOF() + one_dof_rotation_profiler.ModelTag = "prescribedRotation1DOF" + one_dof_rotation_profiler.setRotHat_M(prescribed_rot_axis_M) + one_dof_rotation_profiler.setThetaDDotMax(prescribed_ang_accel_max) + one_dof_rotation_profiler.setThetaInit(prescribed_theta_init) + one_dof_rotation_profiler.setCoastOptionBangDuration(0.75) + one_dof_rotation_profiler.setSmoothingDuration(0.75) + sc_sim.AddModelToTask(dyn_task_name, one_dof_rotation_profiler) + + # Create prescribed platform rotational motion reference message + prescribed_rotation_msg_data = messaging.HingedRigidBodyMsgPayload() + prescribed_rotation_msg_data.theta = prescribed_theta_ref + prescribed_rotation_msg_data.thetaDot = 0.0 + prescribed_rotation_msg = messaging.HingedRigidBodyMsg().write(prescribed_rotation_msg_data) + one_dof_rotation_profiler.spinningBodyInMsg.subscribeTo(prescribed_rotation_msg) + prescribed_platform.prescribedRotationInMsg.subscribeTo(one_dof_rotation_profiler.prescribedRotationOutMsg) + + # Create prescribed platform translational motion profiler + one_dof_translation_profiler = prescribedLinearTranslation.PrescribedLinearTranslation() + one_dof_translation_profiler.ModelTag = "prescribedLinearTranslation" + one_dof_translation_profiler.setTransHat_M(prescribed_trans_axis_M) + one_dof_translation_profiler.setTransAccelMax(prescribed_trans_accel_max) + one_dof_translation_profiler.setTransPosInit(depth_prescribed) + one_dof_translation_profiler.setCoastOptionBangDuration(1.0) + one_dof_translation_profiler.setSmoothingDuration(1.0) + sc_sim.AddModelToTask(dyn_task_name, one_dof_translation_profiler) + + # Create prescribed array boom translational motion reference message + prescribed_translation_msg_data = messaging.LinearTranslationRigidBodyMsgPayload() + prescribed_translation_msg_data.rho = prescribed_pos_ref + prescribed_translation_msg_data.rhoDot = 0.0 + prescribed_translation_msg = messaging.LinearTranslationRigidBodyMsg().write(prescribed_translation_msg_data) + one_dof_translation_profiler.linearTranslationRigidBodyInMsg.subscribeTo(prescribed_translation_msg) + prescribed_platform.prescribedTranslationInMsg.subscribeTo(one_dof_translation_profiler.prescribedTranslationOutMsg) + + # Shared translating body parameters + mass_teles = 50.0 # [kg] + length_teles = 0.2 # [m] + width_teles = 0.2 # [m] + depth_teles = 0.5 # [m] + I_teles_11 = (1 / 12) * mass_teles * (length_teles * length_teles + depth_teles * depth_teles) # [kg m^2] + I_teles_22 = (1 / 12) * mass_teles * (length_teles * length_teles + width_teles * width_teles) # [kg m^2] + I_teles_33 = (1 / 12) * mass_teles * (width_teles * width_teles + depth_teles * depth_teles) # [kg m^2] + I_teles_FcF = [[I_teles_11, 0.0, 0.0], [0.0, I_teles_22, 0.0], [0.0, 0.0, I_teles_33]] # [kg m^2] + teles_rho_init = 0.0 + teles_rho_dot_init = 0.0 + r_FcF_F = np.array([0.5 * depth_teles, 0.0, 0.0]) + telescoping_cant_angle = 60 * macros.D2R + + # Compute r_FP_P for each telescoping body + spacing_distance = 0.25 + l = spacing_distance - 0.5 * width_teles * np.cos(telescoping_cant_angle) + r_F01P_P = np.array([l * np.cos(30 * macros.D2R), + l * np.sin(30 * macros.D2R), + -(0.5 * width_teles * np.sin(telescoping_cant_angle))]) + r_F02P_P = np.array([0, + -l, + -(0.5 * width_teles * np.sin(telescoping_cant_angle))]) + r_F03P_P = np.array([-l * np.cos(30 * macros.D2R), + l * np.sin(30 * macros.D2R), + -(0.5 * width_teles * np.sin(telescoping_cant_angle))]) + + # Compute dcm_FP for telescoping body 1 + theta_F1IntP = 30 * macros.D2R + dcm_F1IntP = np.array([[np.cos(theta_F1IntP), np.sin(theta_F1IntP), 0.0], + [-np.sin(theta_F1IntP), np.cos(theta_F1IntP), 0.0], + [0.0, 0.0, 1.0]]) + theta_F1F1Int = telescoping_cant_angle + dcm_F1F1Int = np.array([[np.cos(theta_F1F1Int), 0.0, -np.sin(theta_F1F1Int)], + [0.0, 1.0, 0.0], + [np.sin(theta_F1F1Int), 0.0, np.cos(theta_F1F1Int)]]) + dcm_F1P = dcm_F1F1Int @ dcm_F1IntP + dcm_PF1 = dcm_F1P.T + + # Compute dcm_FP for telescoping body 2 + theta_F2IntP = -90 * macros.D2R + dcm_F2IntP = np.array([[np.cos(theta_F2IntP), np.sin(theta_F2IntP), 0.0], + [-np.sin(theta_F2IntP), np.cos(theta_F2IntP), 0.0], + [0.0, 0.0, 1.0]]) + theta_F2F2Int = telescoping_cant_angle + dcm_F2F2Int = np.array([[np.cos(theta_F2F2Int), 0.0, -np.sin(theta_F2F2Int)], + [0.0, 1.0, 0.0], + [np.sin(theta_F2F2Int), 0.0, np.cos(theta_F2F2Int)]]) + dcm_F2P = dcm_F2F2Int @ dcm_F2IntP + dcm_PF2 = dcm_F2P.T + + # Compute dcm_FP for telescoping body 3 + theta_F3IntP = 150 * macros.D2R + dcm_F3IntP = np.array([[np.cos(theta_F3IntP), np.sin(theta_F3IntP), 0.0], + [-np.sin(theta_F3IntP), np.cos(theta_F3IntP), 0.0], + [0.0, 0.0, 1.0]]) + theta_F3F3Int = telescoping_cant_angle + dcm_F3F3Int = np.array([[np.cos(theta_F3F3Int), 0.0, -np.sin(theta_F3F3Int)], + [0.0, 1.0, 0.0], + [np.sin(theta_F3F3Int), 0.0, np.cos(theta_F3F3Int)]]) + dcm_F3P = dcm_F3F3Int @ dcm_F3IntP + dcm_PF3 = dcm_F3P.T + + # Compute axis of translation for each telescoping body + f_hat_F = np.array([1.0, 0.0, 0.0]) + f1_hat_P = dcm_PF1 @ f_hat_F + f2_hat_P = dcm_PF2 @ f_hat_F + f3_hat_P = dcm_PF3 @ f_hat_F + + # Create the translating bodies + num_teles_struts = 3 + teles_strut_list = list() + for idx in range(num_teles_struts): + strut_num = idx + 1 + teles_strut_list.append(linearTranslationOneDOFStateEffector.LinearTranslationOneDOFStateEffector()) + teles_strut_list[idx].ModelTag = "translatingBody" + str(strut_num) + teles_strut_list[idx].setMass(mass_teles) + teles_strut_list[idx].setK(8) + teles_strut_list[idx].setC(8) + teles_strut_list[idx].setRhoInit(teles_rho_init) + teles_strut_list[idx].setRhoDotInit(teles_rho_dot_init) + teles_strut_list[idx].setR_FcF_F(r_FcF_F) + teles_strut_list[idx].setIPntFc_F(I_teles_FcF) + sc_sim.AddModelToTask(dyn_task_name, teles_strut_list[idx]) + + # Connect the trusses to the prescribed platform + prescribed_platform.addStateEffector(teles_strut_list[idx]) + + teles_strut_list[0].setFHat_B(f1_hat_P) + teles_strut_list[1].setFHat_B(f2_hat_P) + teles_strut_list[2].setFHat_B(f3_hat_P) + + teles_strut_list[0].setR_F0B_B(r_F01P_P) + teles_strut_list[1].setR_F0B_B(r_F02P_P) + teles_strut_list[2].setR_F0B_B(r_F03P_P) + + teles_strut_list[0].setDCM_FB(dcm_F1P) + teles_strut_list[1].setDCM_FB(dcm_F2P) + teles_strut_list[2].setDCM_FB(dcm_F3P) + + # Set up data logging + sc_state_data_log = sc_object.scStateOutMsg.recorder() + prescribed_translation_data_log = prescribed_platform.prescribedTranslationOutMsg.recorder() + prescribed_rotation_data_log = prescribed_platform.prescribedRotationOutMsg.recorder() + one_dof_rotation_profiler_data_log = one_dof_rotation_profiler.spinningBodyOutMsg.recorder() + teles_strut_1_data_log = teles_strut_list[0].translatingBodyOutMsg.recorder() + teles_strut_2_data_log = teles_strut_list[1].translatingBodyOutMsg.recorder() + teles_strut_3_data_log = teles_strut_list[2].translatingBodyOutMsg.recorder() + sc_sim.AddModelToTask(dyn_task_name, sc_state_data_log) + sc_sim.AddModelToTask(dyn_task_name, prescribed_translation_data_log) + sc_sim.AddModelToTask(dyn_task_name, prescribed_rotation_data_log) + sc_sim.AddModelToTask(dyn_task_name, one_dof_rotation_profiler_data_log) + sc_sim.AddModelToTask(dyn_task_name, teles_strut_1_data_log) + sc_sim.AddModelToTask(dyn_task_name, teles_strut_2_data_log) + sc_sim.AddModelToTask(dyn_task_name, teles_strut_3_data_log) + + # Add Vizard + sc_body_list = [sc_object] + sc_body_list.append(["prescribedPlatform", prescribed_platform.prescribedMotionConfigLogOutMsg]) + for idx in range(num_teles_struts): + sc_body_list.append(["translatingBody" + str(idx + 1), teles_strut_list[idx].translatingBodyConfigLogOutMsg]) + + if vizSupport.vizFound: + viz = vizSupport.enableUnityVisualization(sc_sim, data_rec_task_name, sc_body_list, + saveFile=filename + ) + vizSupport.createCustomModel(viz + , simBodiesToModify=[sc_object.ModelTag] + , modelPath="CUBE" + , scale=[width_hub, length_hub, depth_hub] + , color=vizSupport.toRGBA255("gray")) + vizSupport.createCustomModel(viz + , simBodiesToModify=["prescribedPlatform"] + , modelPath="CUBE" + , scale=[width_prescribed, length_prescribed, depth_prescribed] + , color=vizSupport.toRGBA255("green")) + for idx in range(num_teles_struts): + vizSupport.createCustomModel(viz + , simBodiesToModify=["translatingBody" + str(idx + 1)] + , modelPath="CUBE" + , scale=[depth_teles, length_teles, width_teles] + , color=vizSupport.toRGBA255("purple")) + viz.settings.orbitLinesOn = -1 + + # Run the simulation (chunk 1 is only prescribed motion) + sc_sim.InitializeSimulation() + sim_time_1 = 90.0 # [s] + sc_sim.ConfigureStopTime(macros.sec2nano(sim_time_1)) + sc_sim.ExecuteSimulation() + + # Create telescoping strut 1 reference message + strut_1_rho_ref = 0.2 # [m] + linear_translation_1_msg_data = messaging.LinearTranslationRigidBodyMsgPayload() + linear_translation_1_msg_data.rho = strut_1_rho_ref + linear_translation_1_msg_data.rhoDot = 0.0 + linear_translation_1_msg = messaging.LinearTranslationRigidBodyMsg().write(linear_translation_1_msg_data, macros.sec2nano(sim_time_1)) + teles_strut_list[0].translatingBodyRefInMsg.subscribeTo(linear_translation_1_msg) + + # Create telescoping strut 2 reference message + strut_2_rho_ref = 0.1 # [m] + linear_translation_2_msg_data = messaging.LinearTranslationRigidBodyMsgPayload() + linear_translation_2_msg_data.rho = strut_2_rho_ref + linear_translation_2_msg_data.rhoDot = 0.0 + linear_translation_2_msg = messaging.LinearTranslationRigidBodyMsg().write(linear_translation_2_msg_data, macros.sec2nano(sim_time_1)) + teles_strut_list[1].translatingBodyRefInMsg.subscribeTo(linear_translation_2_msg) + + # Create telescoping strut 3 reference message + strut_3_rho_ref = 0.15 # [m] + linear_translation_3_msg_data = messaging.LinearTranslationRigidBodyMsgPayload() + linear_translation_3_msg_data.rho = strut_3_rho_ref + linear_translation_3_msg_data.rhoDot = 0.0 + linear_translation_3_msg = messaging.LinearTranslationRigidBodyMsg().write(linear_translation_3_msg_data, macros.sec2nano(sim_time_1)) + teles_strut_list[2].translatingBodyRefInMsg.subscribeTo(linear_translation_3_msg) + + # Run the simulation (chunk 2 is the translating body motion only) + sim_time_2 = 60.0 # [s] + sc_sim.ConfigureStopTime(macros.sec2nano(sim_time_1 + sim_time_2)) + sc_sim.ExecuteSimulation() + + # Extract the logged variables + timespan = sc_state_data_log.times() * macros.NANO2SEC # [s] + r_BN_N = sc_state_data_log.r_BN_N # [m] + sigma_BN = sc_state_data_log.sigma_BN + omega_BN_B = sc_state_data_log.omega_BN_B * macros.R2D # [deg/s] + r_PM_M = prescribed_translation_data_log.r_PM_M # [m] + r_prime_PM_M = prescribed_translation_data_log.rPrime_PM_M # [m/s] + r_prime_prime_PM_M = prescribed_translation_data_log.rPrimePrime_PM_M # [m/s^2] + prescribed_theta = macros.R2D * one_dof_rotation_profiler_data_log.theta # [deg] + omega_PM_P = prescribed_rotation_data_log.omega_PM_P * macros.R2D # [deg/s] + omega_prime_PM_P = prescribed_rotation_data_log.omegaPrime_PM_P * macros.R2D # [deg/s^2] + teles_strut_1_rho = teles_strut_1_data_log.rho # [m] + teles_strut_2_rho = teles_strut_2_data_log.rho # [m] + teles_strut_3_rho = teles_strut_3_data_log.rho # [m] + teles_strut_1_rho_dot = teles_strut_1_data_log.rhoDot # [m/s] + teles_strut_2_rho_dot = teles_strut_2_data_log.rhoDot # [m/s] + teles_strut_3_rho_dot = teles_strut_3_data_log.rhoDot # [m/s] + + figure_list = {} + plt.close("all") + + # Plot prescribed platform displacements + prescribed_rho = np.dot(r_PM_M, prescribed_trans_axis_M) # [m] + fig1, ax1 = plt.subplots() + ax1.plot(timespan, prescribed_theta, label=r"$\theta_{\text{P}}$", color="teal") + ax1.tick_params(axis="y", labelcolor="teal") + ax1.set_xlabel("Time (s)", fontsize=16) + ax1.set_ylabel("(deg)", color="teal", fontsize=16) + ax2 = ax1.twinx() + ax2.plot(timespan[1:], 100 * prescribed_rho[1:], label=r'$\rho_{\text{P}}$', color="darkviolet") + ax2.set_ylabel("(cm)", color="darkviolet", fontsize=16) + ax2.tick_params(axis="y", labelcolor="darkviolet") + handles_ax1, labels_ax1 = ax1.get_legend_handles_labels() + handles_ax2, labels_ax2 = ax2.get_legend_handles_labels() + handles = handles_ax1 + handles_ax2 + labels = labels_ax1 + labels_ax2 + plt.legend(handles=handles, labels=labels, loc="center right", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_1" + figure_list[plt_name] = plt.figure(1) + + # Plot prescribed platform velocities + prescribed_theta_dot = np.dot(omega_PM_P, prescribed_rot_axis_M) # [deg/s] + prescribed_rho_dot = np.dot(r_prime_PM_M, prescribed_trans_axis_M) # [m/s] + fig2, ax1 = plt.subplots() + ax1.plot(timespan, prescribed_theta_dot, label=r"$\dot{\theta}_{\text{P}}$", color="teal") + ax1.tick_params(axis="y", labelcolor="teal") + ax1.set_xlabel("Time (s)", fontsize=16) + ax1.set_ylabel("(deg/s)", color="teal", fontsize=16) + ax2 = ax1.twinx() + plt.plot(timespan, 100 * prescribed_rho_dot, label=r'$\dot{\rho}_{\text{P}}$', color="darkviolet") + ax2.set_ylabel("(cm/s)", color="darkviolet", fontsize=16) + ax2.tick_params(axis="y", labelcolor="darkviolet") + handles_ax1, labels_ax1 = ax1.get_legend_handles_labels() + handles_ax2, labels_ax2 = ax2.get_legend_handles_labels() + handles = handles_ax1 + handles_ax2 + labels = labels_ax1 + labels_ax2 + plt.legend(handles=handles, labels=labels, loc="center right", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_2" + figure_list[plt_name] = plt.figure(2) + + # Plot prescribed platform accelerations + prescribed_theta_ddot = np.dot(omega_prime_PM_P, prescribed_rot_axis_M) # [deg/s^2] + prescribed_rho_ddot = np.dot(r_prime_prime_PM_M, prescribed_trans_axis_M) # [m/s^2] + fig3, ax1 = plt.subplots() + ax1.plot(timespan, prescribed_theta_ddot, label=r"$\ddot{\theta}_{\text{P}}$", color="teal") + ax1.tick_params(axis="y", labelcolor="teal") + ax1.set_xlabel("Time (s)", fontsize=16) + ax1.set_ylabel("(deg/$s^2$)", color="teal", fontsize=16) + ax2 = ax1.twinx() + ax2.plot(timespan, 100.0 * prescribed_rho_ddot, label=r"$\ddot{\rho}_{\text{P}}$", color="darkviolet") + ax2.set_ylabel("(cm/$s^2$)", color="darkviolet", fontsize=16) + ax2.tick_params(axis="y", labelcolor="darkviolet") + handles_ax1, labels_ax1 = ax1.get_legend_handles_labels() + handles_ax2, labels_ax2 = ax2.get_legend_handles_labels() + handles = handles_ax1 + handles_ax2 + labels = labels_ax1 + labels_ax2 + plt.legend(handles=handles, labels=labels, loc="upper right", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_3" + figure_list[plt_name] = plt.figure(3) + + # Plot telescoping strut displacements + plt.figure(4) + plt.clf() + plt.plot(timespan, 100 * teles_strut_1_rho, label=r'$\rho_{T_1}$', color="teal") + plt.plot(timespan, 100 * teles_strut_2_rho, label=r'$\rho_{T_2}$', color="darkviolet") + plt.plot(timespan, 100 * teles_strut_3_rho, label=r'$\rho_{T_3}$', color="blue") + plt.title(r'Telescoping Strut Displacements', fontsize=16) + plt.ylabel('(cm)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc="center left", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_4" + figure_list[plt_name] = plt.figure(4) + + # Plot telescoping strut displacement rates + plt.figure(5) + plt.clf() + plt.plot(timespan, 100 * teles_strut_1_rho_dot, label=r'$\dot{\rho}_{T_1}$', color="teal") + plt.plot(timespan, 100 * teles_strut_2_rho_dot, label=r'$\dot{\rho}_{T_2}$', color="darkviolet") + plt.plot(timespan, 100 * teles_strut_3_rho_dot, label=r'$\dot{\rho}_{T_3}$', color="blue") + plt.title(r'Telescoping Strut Displacement Rates', fontsize=16) + plt.ylabel('(cm/s)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc="upper right", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_5" + figure_list[plt_name] = plt.figure(5) + + # Plot hub inertial position + plt.figure(6) + plt.clf() + plt.plot(timespan, r_BN_N[:, 0], label=r'$r_1$', color="teal") + plt.plot(timespan, r_BN_N[:, 1], label=r'$r_2$', color="darkviolet") + plt.plot(timespan, r_BN_N[:, 2], label=r'$r_3$', color="blue") + plt.title(r'Hub Inertial Position ${}^\mathcal{N} r_{B/N}$', fontsize=16) + plt.ylabel('(m)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc="center right", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_6" + figure_list[plt_name] = plt.figure(6) + + # Plot hub inertial attitude + plt.figure(7) + plt.clf() + plt.plot(timespan, sigma_BN[:, 0], label=r'$\sigma_1$', color="teal") + plt.plot(timespan, sigma_BN[:, 1], label=r'$\sigma_2$', color="darkviolet") + plt.plot(timespan, sigma_BN[:, 2], label=r'$\sigma_3$', color="blue") + plt.title(r'Hub Inertial Attitude $\sigma_{\mathcal{B}/\mathcal{N}}$', fontsize=16) + plt.ylabel('', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc="center right", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_7" + figure_list[plt_name] = plt.figure(7) + + # Plot hub inertial angular velocity + plt.figure(8) + plt.clf() + plt.plot(timespan, omega_BN_B[:, 0], label=r'$\omega_1$', color="teal") + plt.plot(timespan, omega_BN_B[:, 1], label=r'$\omega_2$', color="darkviolet") + plt.plot(timespan, omega_BN_B[:, 2], label=r'$\omega_3$', color="blue") + plt.title(r'Hub Inertial Angular Velocity ${}^\mathcal{B} \omega_{\mathcal{B}/\mathcal{N}}$', fontsize=16) + plt.ylabel('(deg/s)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc="lower right", prop={"size": 16}) + plt.grid(True) + plt_name = filename + "_8" + figure_list[plt_name] = plt.figure(8) + + if show_plots: + plt.show() + plt.close("all") + + return figure_list + + +if __name__ == "__main__": + run(True) diff --git a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp index 2d1de789f7..a967ed2ffc 100755 --- a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp @@ -68,6 +68,16 @@ void StateEffector::updateContributions(double integTime, BackSubMatrices & back return; } +/*! This method must be implemented to attach a state effector to a prescribed motion state effector. The additional + coupling terms required to attach the state effector to the prescribed component are computed in this method. Similar + to how the spacecraft module calls updateContributions for all its attached effectors, the prescribed motion module + calls this method for all its attached state effectors. + */ +void StateEffector::addPrescribedMotionCouplingContributions(BackSubMatrices& backSubContr) +{ + return; +} + /*! This method allows for an individual stateEffector to add its energy and momentum calculations to the dynamicObject. The analytical devlopement of these contributions can be seen in Basilisk/simulation/dynamics/_Documentation/Basilisk-EnergyAndMomentum-20161219.pdf*/ @@ -95,6 +105,18 @@ void StateEffector::writeOutputStateMessages(uint64_t integTimeNanos) return; } +/*! This method allows the state effector to register its properties */ +void StateEffector::registerProperties(DynParamManager& states) +{ + return; +} + +/*! This method allows the state effector to link in prescribed motion properties */ +void StateEffector::linkInPrescribedMotionProperties(DynParamManager& properties) +{ + return; +} + /*! This method ensures that stateEffectors can be implemented using the multi-spacecraft archticture */ void StateEffector::prependSpacecraftNameToStates() { @@ -240,3 +262,63 @@ void StateEffector::setPropName_vehicleGravity(std::string value) bskLogger.bskLog(BSK_ERROR, "StateEffector: propName_vehicleGravity variable must be a non-empty string"); } } + +void StateEffector::setPropName_prescribedPosition(std::string value) +{ + // check that value is acceptable + if (!value.empty()) { + this->propName_prescribedPosition = value; + } else { + bskLogger.bskLog(BSK_ERROR, "StateEffector: propName_prescribedPosition variable must be a non-empty string"); + } +} + +void StateEffector::setPropName_prescribedVelocity(std::string value) +{ + // check that value is acceptable + if (!value.empty()) { + this->propName_prescribedVelocity = value; + } else { + bskLogger.bskLog(BSK_ERROR, "StateEffector: propName_prescribedVelocity variable must be a non-empty string"); + } +} + +void StateEffector::setPropName_prescribedAcceleration(std::string value) +{ + // check that value is acceptable + if (!value.empty()) { + this->propName_prescribedAcceleration = value; + } else { + bskLogger.bskLog(BSK_ERROR, "StateEffector: propName_prescribedAcceleration variable must be a non-empty string"); + } +} + +void StateEffector::setPropName_prescribedAttitude(std::string value) +{ + // check that value is acceptable + if (!value.empty()) { + this->propName_prescribedAttitude = value; + } else { + bskLogger.bskLog(BSK_ERROR, "StateEffector: propName_prescribedAttitude variable must be a non-empty string"); + } +} + +void StateEffector::setPropName_prescribedAngVelocity(std::string value) +{ + // check that value is acceptable + if (!value.empty()) { + this->propName_prescribedAngVelocity = value; + } else { + bskLogger.bskLog(BSK_ERROR, "StateEffector: propName_prescribedAngVelocity variable must be a non-empty string"); + } +} + +void StateEffector::setPropName_prescribedAngAcceleration(std::string value) +{ + // check that value is acceptable + if (!value.empty()) { + this->propName_prescribedAngAcceleration = value; + } else { + bskLogger.bskLog(BSK_ERROR, "StateEffector: propName_prescribedAngAcceleration variable must be a non-empty string"); + } +} diff --git a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h index a5d1c6b777..2a8baea7b3 100755 --- a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h @@ -116,6 +116,30 @@ class StateEffector { void setPropName_vehicleGravity(std::string value); /** getter for `propName_vehicleGravity` property */ const std::string getPropName_vehicleGravity() const { return this->propName_vehicleGravity; } + /** setter for `propName_prescribedPosition` property */ + void setPropName_prescribedPosition(std::string value); + /** getter for `propName_prescribedPosition` property */ + const std::string getPropName_prescribedPosition() const { return this->propName_prescribedPosition; } + /** setter for `propName_prescribedVelocity` property */ + void setPropName_prescribedVelocity(std::string value); + /** getter for `propName_prescribedVelocity` property */ + const std::string getPropName_prescribedVelocity() const { return this->propName_prescribedVelocity; } + /** setter for `propName_prescribedAcceleration` property */ + void setPropName_prescribedAcceleration(std::string value); + /** getter for `propName_prescribedAcceleration` property */ + const std::string getPropName_prescribedAcceleration() const { return this->propName_prescribedAcceleration; } + /** setter for `propName_prescribedAttitude` property */ + void setPropName_prescribedAttitude(std::string value); + /** getter for `propName_prescribedAttitude` property */ + const std::string getPropName_prescribedAttitude() const { return this->propName_prescribedAttitude; } + /** setter for `propName_prescribedAngVelocity` property */ + void setPropName_prescribedAngVelocity(std::string value); + /** getter for `propName_prescribedAngVelocity` property */ + const std::string getPropName_prescribedAngVelocity() const { return this->propName_prescribedAngVelocity; } + /** setter for `propName_prescribedAngAcceleration` property */ + void setPropName_prescribedAngAcceleration(std::string value); + /** getter for `propName_prescribedAngAcceleration` property */ + const std::string getPropName_prescribedAngAcceleration() const { return this->propName_prescribedAngAcceleration; } BSKLogger bskLogger; //!< BSK Logging @@ -124,13 +148,16 @@ class StateEffector { virtual ~StateEffector(); //!< Destructor virtual void updateEffectorMassProps(double integTime); //!< Method for stateEffector to give mass contributions virtual void updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N); //!< Back-sub contributions + virtual void addPrescribedMotionCouplingContributions(BackSubMatrices& backSubContr); //!< Method for adding coupling contributions for state effector branching on prescribed motion virtual void updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, double & rotEnergyContr, Eigen::Vector3d omega_BN_B); //!< Energy and momentum calculations virtual void modifyStates(double integTime); //!< Modify state values after integration virtual void calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B); //!< Force and torque on s/c due to stateEffector virtual void writeOutputStateMessages(uint64_t integTimeNanos); //!< Write State Messages after integration virtual void registerStates(DynParamManager& states) = 0; //!< Method for stateEffectors to register states + virtual void registerProperties(DynParamManager& states); //!< Method for stateEffectors to register properties virtual void linkInStates(DynParamManager& states) = 0; //!< Method for stateEffectors to get other states + virtual void linkInPrescribedMotionProperties(DynParamManager& properties); //!< Method for stateEffectors to access prescribed motion properties virtual void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN)=0; //!< Method for each stateEffector to calculate derivatives virtual void prependSpacecraftNameToStates(); virtual void receiveMotherSpacecraftData(Eigen::Vector3d rSC_BP_P, Eigen::Matrix3d dcmSC_BP); //!< class method @@ -152,6 +179,12 @@ class StateEffector { std::string propName_inertialVelocity = ""; //!< property name of inertialVelocity std::string propName_vehicleGravity = ""; //!< property name of vehicleGravity + std::string propName_prescribedPosition = ""; //!< property name of prescribedPosition + std::string propName_prescribedVelocity = ""; //!< property name of prescribedVelocity + std::string propName_prescribedAcceleration = ""; //!< property name of prescribedAcceleration + std::string propName_prescribedAttitude = ""; //!< property name of prescribedAttitude + std::string propName_prescribedAngVelocity = ""; //!< property name of prescribedAngVelocity + std::string propName_prescribedAngAcceleration = ""; //!< property name of prescribedAngAcceleration }; diff --git a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.cpp b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.cpp index f4a6186433..556f3c6046 100644 --- a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.cpp +++ b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.cpp @@ -78,11 +78,29 @@ void LinearTranslationOneDOFStateEffector::setC(double c) { void LinearTranslationOneDOFStateEffector::linkInStates(DynParamManager& statesIn) { - this->inertialPositionProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialPosition); - this->inertialVelocityProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialVelocity); + // Get access to the hub's states needed for dynamic coupling + this->hubOmega = statesIn.getStateObject("hubOmega"); + + // Get access to properties needed for dynamic coupling (Hub or prescribed) + this->inertialPositionProperty = statesIn.getPropertyReference(this->propName_inertialPosition); + this->inertialVelocityProperty = statesIn.getPropertyReference(this->propName_inertialVelocity); this->g_N = statesIn.getPropertyReference("g_N"); } +/*! This method is used to link properties + @return void + @param properties The parameter manager to collect from + */ +void LinearTranslationOneDOFStateEffector::linkInPrescribedMotionProperties(DynParamManager& properties) +{ + this->prescribedPositionProperty = properties.getPropertyReference(this->propName_prescribedPosition); + this->prescribedVelocityProperty = properties.getPropertyReference(this->propName_prescribedVelocity); + this->prescribedAccelerationProperty = properties.getPropertyReference(this->propName_prescribedAcceleration); + this->prescribedAttitudeProperty = properties.getPropertyReference(this->propName_prescribedAttitude); + this->prescribedAngVelocityProperty = properties.getPropertyReference(this->propName_prescribedAngVelocity); + this->prescribedAngAccelerationProperty = properties.getPropertyReference(this->propName_prescribedAngAcceleration); +} + void LinearTranslationOneDOFStateEffector::registerStates(DynParamManager& states) { this->rhoState = states.registerState(1, 1, nameOfRhoState); @@ -198,20 +216,89 @@ void LinearTranslationOneDOFStateEffector::computeBackSubContributions(BackSubMa return; } - this->aRho = - this->fHat_B.transpose(); + this->aRho = -this->fHat_B.transpose(); this->bRho = this->fHat_B.transpose() * this->rTilde_FcB_B; this->cRho = 1.0 / this->mass * (this->motorForce - this->k * (this->rho - this->rhoRef) - - this->c * (this->rhoDot - this->rhoDotRef) - + this->fHat_B.transpose() * (F_g - this->mass * (2 * this->omegaTilde_BN_B * this->rPrime_FcB_B - + this->omegaTilde_BN_B*this->omegaTilde_BN_B*this->r_FcB_B))); + - this->c * (this->rhoDot - this->rhoDotRef) + + this->fHat_B.transpose() * + (F_g - this->mass * (2 * this->omegaTilde_BN_B * this->rPrime_FcB_B + + this->omegaTilde_BN_B * this->omegaTilde_BN_B * + this->r_FcB_B))); - backSubContr.matrixA = this->mass * this->fHat_B * this->aRho.transpose(); + backSubContr.matrixA = this->mass * this->fHat_B * this->aRho.transpose(); backSubContr.matrixB = this->mass * this->fHat_B * this->bRho.transpose(); backSubContr.matrixC = this->mass * this->rTilde_FcB_B * this->fHat_B * this->aRho.transpose(); - backSubContr.matrixD = this->mass * this->rTilde_FcB_B * this->fHat_B * this->bRho.transpose(); - backSubContr.vecTrans = - this->mass * this->cRho * this->fHat_B; - backSubContr.vecRot = - this->mass * this->omegaTilde_BN_B * this->rTilde_FcB_B * this->rPrime_FcB_B - - this->mass * this->cRho * this->rTilde_FcB_B * this->fHat_B; + backSubContr.matrixD = this->mass * this->rTilde_FcB_B * this->fHat_B * this->bRho.transpose(); + backSubContr.vecTrans = -this->mass * this->cRho * this->fHat_B; + backSubContr.vecRot = -this->mass * this->omegaTilde_BN_B * this->rTilde_FcB_B * this->rPrime_FcB_B + - this->mass * this->cRho * this->rTilde_FcB_B * this->fHat_B; +} + +void LinearTranslationOneDOFStateEffector::addPrescribedMotionCouplingContributions(BackSubMatrices & backSubContr) { + + // Access prescribed motion properties + Eigen::Vector3d r_PB_B = (Eigen::Vector3d)*this->prescribedPositionProperty; + Eigen::Vector3d rPrime_PB_B = (Eigen::Vector3d)*this->prescribedVelocityProperty; + Eigen::Vector3d rPrimePrime_PB_B = (Eigen::Vector3d)*this->prescribedAccelerationProperty; + Eigen::MRPd sigma_PB; + sigma_PB = (Eigen::Vector3d)*this->prescribedAttitudeProperty; + Eigen::Vector3d omega_PB_P = (Eigen::Vector3d)*this->prescribedAngVelocityProperty; + Eigen::Vector3d omegaPrime_PB_P = (Eigen::Vector3d)*this->prescribedAngAccelerationProperty; + Eigen::Matrix3d dcm_PB = sigma_PB.toRotationMatrix().transpose(); + + // Collect hub states + Eigen::Vector3d omega_bN_b = this->hubOmega->getState(); + Eigen::Vector3d omega_BN_P = dcm_PB * omega_bN_b; + + // Prescribed motion translation coupling contributions + Eigen::Vector3d fHat_P = this->fHat_B; + Eigen::Vector3d r_PB_P = dcm_PB * r_PB_B; + Eigen::Matrix3d rTilde_PB_P = eigenTilde(r_PB_P); + backSubContr.matrixB += - this->mass * fHat_P * this->aRho.transpose() * rTilde_PB_P; + + Eigen::Matrix3d omegaTilde_PB_P = eigenTilde(omega_PB_P); + Eigen::Vector3d rPPrime_FcP_P = this->rPrime_FcB_B; + Eigen::Matrix3d omegaPrimeTilde_PB_P = eigenTilde(omegaPrime_PB_P); + Eigen::Vector3d r_FcP_P = this->r_FcB_B; + Eigen::Vector3d rPrimePrime_PB_P = dcm_PB * rPrimePrime_PB_B; + Eigen::Matrix3d omegaTilde_BN_P = eigenTilde(omega_BN_P); + Eigen::Vector3d rPrime_PB_P = dcm_PB * rPrime_PB_B; + Eigen::Vector3d term1 = 2.0 * omegaTilde_PB_P * rPPrime_FcP_P + + omegaPrimeTilde_PB_P * r_FcP_P + + omegaTilde_PB_P * omegaTilde_PB_P * r_FcP_P + + rPrimePrime_PB_P; + Eigen::Vector3d term2 = rPrimePrime_PB_P + 2.0 * omegaTilde_BN_P * rPrime_PB_P + + omegaTilde_BN_P * omegaTilde_BN_P * r_PB_P; + Eigen::Vector3d term3 = omegaPrime_PB_P + omegaTilde_BN_P * omega_PB_P; + backSubContr.vecTrans += - this->mass * term1 + - this->mass * this->aRho.transpose() * term2 * fHat_P + - this->mass * this->bRho.transpose() * term3 * fHat_P; + + // Prescribed motion rotation coupling contributions + backSubContr.matrixC += this->mass * rTilde_PB_P * fHat_P * this->aRho.transpose(); + + Eigen::Vector3d r_FcB_P = r_FcP_P + r_PB_P; + Eigen::Matrix3d rTilde_FcB_P = eigenTilde(r_FcB_P); + backSubContr.matrixD += + this->mass * rTilde_PB_P * fHat_P * this->bRho.transpose() + - this->mass * rTilde_FcB_P * fHat_P * this->aRho.transpose() * rTilde_PB_P; + + Eigen::Matrix3d IPntFc_P = this->IPntFc_B; + Eigen::Vector3d omega_PN_P = omega_PB_P + omega_BN_P; + Eigen::Matrix3d omegaTilde_PN_P = eigenTilde(omega_PN_P); + Eigen::Matrix3d rTilde_FcP_P = eigenTilde(r_FcP_P); + + Eigen::Vector3d vecRotTerm1 = - IPntFc_P * omegaPrime_PB_P - omegaTilde_PN_P * IPntFc_P * omega_PB_P; + Eigen::Vector3d vecRotTerm2 = - this->mass * rTilde_FcB_P * term1; + Eigen::Vector3d vecRotTerm3 = - this->mass * (omegaTilde_BN_P * rTilde_PB_P - omegaTilde_PB_P * rTilde_FcP_P) * rPPrime_FcP_P; + - this->mass * omegaTilde_BN_P * rTilde_FcB_P * (omegaTilde_PB_P * r_FcP_P + rPrime_PB_P); + Eigen::Vector3d vecRotTerm4 = - this->mass * this->cRho * rTilde_PB_P * fHat_P; + Eigen::Vector3d vecRotTerm5 = - this->mass * rTilde_FcB_P * fHat_P * (this->aRho.transpose() * term2) + - this->mass * rTilde_FcB_P * fHat_P * (this->bRho.transpose() * term3); + backSubContr.vecRot += vecRotTerm1 + + vecRotTerm2 + + vecRotTerm3 + + vecRotTerm4 + + vecRotTerm5; } void LinearTranslationOneDOFStateEffector::computeDerivatives(double integTime, @@ -235,15 +322,21 @@ void LinearTranslationOneDOFStateEffector::updateEnergyMomContributions(double i double & rotEnergyContr, Eigen::Vector3d omega_BN_B) { + // Update omega_BN_B and omega_FN_B this->omega_BN_B = omega_BN_B; this->omegaTilde_BN_B = eigenTilde(this->omega_BN_B); Eigen::Vector3d omega_FN_B = this->omega_BN_B; - Eigen::Vector3d rDotFcB_B = this->rPrime_FcB_B + this->omegaTilde_BN_B * this->r_FcB_B; - rotAngMomPntCContr_B = this->IPntFc_B * omega_FN_B + this->mass * this->r_FcB_B.cross(rDotFcB_B); + // Compute rDot_FcB_B + Eigen::Vector3d rDot_FcB_B = this->rPrime_FcB_B + this->omegaTilde_BN_B * this->r_FcB_B; + + // Find rotational angular momentum contribution + rotAngMomPntCContr_B = this->IPntFc_B * omega_FN_B + this->mass * this->r_FcB_B.cross(rDot_FcB_B); + + // Find rotational energy contribution rotEnergyContr = 1.0 / 2.0 * omega_FN_B.dot(this->IPntFc_B * omega_FN_B) - + 1.0 / 2.0 * this->mass * rDotFcB_B.dot(rDotFcB_B) - + 1.0 / 2.0 * this->k * (this->rho - this->rhoRef) * (this->rho - this->rhoRef); + + 1.0 / 2.0 * this->mass * rDot_FcB_B.dot(rDot_FcB_B) + + 1.0 / 2.0 * this->k * (this->rho - this->rhoRef) * (this->rho - this->rhoRef); } void LinearTranslationOneDOFStateEffector::computeTranslatingBodyInertialStates() diff --git a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.h b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.h index ff29ad46cd..5dcef4fb01 100644 --- a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.h +++ b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.h @@ -140,6 +140,7 @@ class LinearTranslationOneDOFStateEffector : void Reset(uint64_t CurrentClock) override; void registerStates(DynParamManager& states) override; void linkInStates(DynParamManager& states) override; + void linkInPrescribedMotionProperties(DynParamManager& states) override; void writeOutputStateMessages(uint64_t CurrentSimNanos) override; void updateEffectorMassProps(double integTime) override; void updateContributions(double integTime, @@ -156,6 +157,17 @@ class LinearTranslationOneDOFStateEffector : void computeTranslatingBodyInertialStates(); void computeBackSubContributions(BackSubMatrices& backSubContr, const Eigen::Vector3d& F_g); void readInputMessages(); + void addPrescribedMotionCouplingContributions(BackSubMatrices& backSubContr) override; //!< Method for adding coupling contributions for state effector branching on prescribed motion + + // Properties required for prescribed motion branching/attachment + StateData* hubOmega; //!< [rad/s] hub inertial angular velocity vector + + Eigen::MatrixXd* prescribedPositionProperty = nullptr; //!< [m] r_PB_B prescribed position relative to hub + Eigen::MatrixXd* prescribedVelocityProperty = nullptr; //!< [m/s] rPrime_PB_B prescribed velocity relative to hub + Eigen::MatrixXd* prescribedAccelerationProperty = nullptr; //!< [m/s^2] rPrimePrime_PB_B prescribed acceleration relative to hub + Eigen::MatrixXd* prescribedAttitudeProperty = nullptr; //!< sigma_PB prescribed MRP attitude relative to hub + Eigen::MatrixXd* prescribedAngVelocityProperty = nullptr; //!< [rad/s] omega_PB_P prescribed angular velocity relative to hub + Eigen::MatrixXd* prescribedAngAccelerationProperty = nullptr; //!< [rad/s^2] omegaPrime_PB_P prescribed angular acceleration relative to hub }; #endif /* LINEAR_TRANSLATION_ONE_DOF_STATE_EFFECTOR_H */ diff --git a/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffector.py b/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffector.py index b127befb5a..c08d61999d 100644 --- a/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffector.py +++ b/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffector.py @@ -1,6 +1,6 @@ # ISC License # -# Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder +# Copyright (c) 2025, 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 @@ -14,14 +14,6 @@ # ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. - -# -# Unit Test Script -# Module Name: prescribedMotion integrated unit test with prescribedRotation1DOF and prescribedLinearTranslation -# Author: Leah Kiner -# Creation Date: Jan 10, 2022 -# - import inspect import os @@ -36,10 +28,9 @@ from Basilisk.simulation import prescribedMotionStateEffector from Basilisk.simulation import prescribedRotation1DOF from Basilisk.simulation import spacecraft +from Basilisk.utilities import RigidBodyKinematics as rbk from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import RigidBodyKinematics as rbk -from Basilisk.utilities import unitTestSupport filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) @@ -48,599 +39,310 @@ matplotlib.rc('xtick', labelsize=16) matplotlib.rc('ytick', labelsize=16) -# Vary the simulation parameters for pytest -@pytest.mark.parametrize("rotTest", [True, False]) -@pytest.mark.parametrize("thetaInit", [0, np.pi/18]) -@pytest.mark.parametrize("theta_Ref", [np.pi/36]) -@pytest.mark.parametrize("posInit", [0, 0.2]) -@pytest.mark.parametrize("posRef", [0.1]) -@pytest.mark.parametrize("accuracy", [1e-8]) -def test_PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posInit, posRef, accuracy): + +@pytest.mark.parametrize("theta_init", [0.0, macros.D2R * 10.0, macros.D2R * -5.0]) +@pytest.mark.parametrize("theta_ref", [0.0, macros.D2R * 5.0]) +@pytest.mark.parametrize("pos_init", [0.0, 0.1, -0.1]) +@pytest.mark.parametrize("pos_ref", [0.0, 0.05]) +def test_prescribed_motion_state_effector(show_plots, + theta_init, + theta_ref, + pos_init, + pos_ref): r""" - **Validation Test Description** - - 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 - 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 rotation for the prescribed body - using the :ref:`prescribedRotation1DOF` flight software module. The second scenario prescribes - linear translation for the prescribed body using the :ref:`prescribedLinearTranslation` flight software module. - - 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 attitude and angular - velocity magnitude are compared with the reference values. This unit test also ensures that the profiled - translational maneuver 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. - Additionally for each scenario, the conservation quantities of orbital angular momentum, rotational angular - momentum, and orbital energy are checked to validate the module dynamics. + **Verification Test Description** + + The unit test for this module is an integrated test with two prescribed motion kinematic profiler modules. + This is required because the prescribed motion module does not define or integrate states for the prescribed body. + Both the :ref:`prescribedRotation1DOF` and :ref:`prescribedLinearTranslation` modules are configured so that + the prescribed body simultaneously translates and rotates relative to the spacecraft hub. The initial attitude + and displacement of the prescribed body are varied relative to the hub. + + The test checks that the conservation quantities of the spacecraft orbital energy, orbital angular momentum, + and rotational angular momentum remain constant over the duration of the simulation. **Test Parameters** Args: - rotTest (bool): (True) Runs the rotational motion test. (False) Runs the translational motion test. - thetaInit (float): [rad] Initial PRV angle of the P frame with respect to the M frame - theta_Ref (float): [rad] Reference PRV angle of the P frame with respect to the M frame - thetaDDotMax (float): [rad/s^2] Maximum angular acceleration for the attitude maneuver - scalarPosInit (float): [m] Initial scalar position of the P frame with respect to the M frame - scalarPosRef (float): [m] Reference scalar position of the P frame with respect to the M frame - scalarAccelMax (float): [m/s^2] Maximum acceleration for the translational maneuver - accuracy (float): absolute accuracy value used in the validation tests + theta_init (float): [rad] Initial PRV angle of the P frame with respect to the M frame + theta_ref (float): [rad] Reference PRV angle of the P frame with respect to the M frame + pos_init (float): [m] Initial displacement of the P frame with respect to the M frame + pos_ref (float): [m] Reference displacement of the P frame with respect to the M frame **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_PM_Pinal`` - and angular velocity magnitude ``thetaDot_Final`` are compared with the reference values ``theta_Ref`` and - ``thetaDot_Ref``, respectively. This unit test also ensures that the profiled translational maneuver is properly - computed for a series of initial and reference positions and maximum accelerations. The final prescribed position - magnitude ``r_PM_M_Final`` and velocity magnitude ``rPrime_PM_M_Final`` are compared with the reference values - ``r_PM_M_Ref`` and ``rPrime_PM_M_Ref``, respectively. Additionally for each scenario, the conservation quantities - of orbital angular momentum, rotational angular momentum, and orbital energy are checked to validate the module - dynamics. + The test checks that the conservation quantities of the spacecraft orbital energy, orbital angular momentum, + and rotational angular momentum remain constant over the duration of the simulation. """ - [testResults, testMessage] = PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posInit, posRef, accuracy) - - assert testResults < 1, testMessage - - -def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posInit, posRef, accuracy): - """Call this routine directly to run the unit test.""" - __tracebackhide__ = True - - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty list to store test log messages - unitTaskName = "unitTask" - unitProcessName = "TestProcess" - - # Create a sim module as an empty container - unitTestSim = SimulationBaseClass.SimBaseClass() - - # Create test thread - testIncrement = 0.1 # [s] - testProcessRate = macros.sec2nano(testIncrement) # update process rate update time - testProc = unitTestSim.CreateNewProcess(unitProcessName) - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - - # Add the spacecraft module to test file - scObject = spacecraft.Spacecraft() - scObject.ModelTag = "spacecraftBody" - - # Define the mass properties of the rigid spacecraft hub - scObject.hub.mHub = 750.0 - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] - scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] - - # Set the initial inertial hub states - scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] - scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] - scObject.hub.omega_BN_BInit = np.array([0.0, 0.0, 0.0]) - scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] - - # Add the scObject to the runtime call list - unitTestSim.AddModelToTask(unitTaskName, scObject) - - # Add the prescribedMotion dynamics module to test file - platform = prescribedMotionStateEffector.PrescribedMotionStateEffector() - - # Define the state effector properties - transAxis_M = np.array([1.0, 0.0, 0.0]) - rotAxis_M = np.array([1.0, 0.0, 0.0]) - r_PM_M = posInit * transAxis_M - prvInit_PM = thetaInit * rotAxis_M - sigma_PM = rbk.PRV2MRP(prvInit_PM) - - platform.mass = 100.0 - platform.IPntPc_P= [[50.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]] - platform.r_MB_B = [0.0, 0.0, 0.0] - platform.r_PcP_P= [0.0, 0.0, 0.0] - platform.r_PM_M = r_PM_M - platform.rPrime_PM_M = np.array([0.0, 0.0, 0.0]) - platform.rPrimePrime_PM_M = np.array([0.0, 0.0, 0.0]) - platform.omega_PM_P= np.array([0.0, 0.0, 0.0]) - platform.omegaPrime_PM_P= np.array([0.0, 0.0, 0.0]) - platform.sigma_PM = sigma_PM - platform.omega_MB_B = [0.0, 0.0, 0.0] - platform.omegaPrime_MB_B = [0.0, 0.0, 0.0] - platform.sigma_MB = [0.0, 0.0, 0.0] - platform.ModelTag = "Platform" - - # Add platform to spacecraft - scObject.addStateEffector(platform) - - # Add the test module to runtime call list - unitTestSim.AddModelToTask(unitTaskName, platform) - - if rotTest: - - # ** ** ** ** ** ROTATIONAL 1 DOF INTEGRATED TEST: ** ** ** ** ** - - # Create an instance of the prescribedRotation1DOF module to be tested - PrescribedRot1DOF = prescribedRotation1DOF.PrescribedRotation1DOF() - PrescribedRot1DOF.ModelTag = "prescribedRotation1DOF" - - # Add the prescribedRotation1DOF test module to runtime call list - unitTestSim.AddModelToTask(unitTaskName, PrescribedRot1DOF) - - # Initialize the prescribedRotation1DOF test module configuration data - accelMax = 0.01 # [rad/s^2] - PrescribedRot1DOF.setRotHat_M(rotAxis_M) - PrescribedRot1DOF.setThetaDDotMax(accelMax) - PrescribedRot1DOF.setThetaInit(thetaInit) - - # Create the prescribedRotation1DOF input message - thetaDot_Ref = 0.0 # [rad/s] - SpinningBodyMessageData = messaging.HingedRigidBodyMsgPayload() - SpinningBodyMessageData.theta = theta_Ref - SpinningBodyMessageData.thetaDot = thetaDot_Ref - SpinningBodyMessage = messaging.HingedRigidBodyMsg().write(SpinningBodyMessageData) - PrescribedRot1DOF.spinningBodyInMsg.subscribeTo(SpinningBodyMessage) - - platform.prescribedRotationInMsg.subscribeTo(PrescribedRot1DOF.prescribedRotationOutMsg) - - # Add Earth gravity to the simulation - earthGravBody = gravityEffector.GravBodyData() - earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 - earthGravBody.isCentralBody = True - scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) - - # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"]) - unitTestSim.AddModelToTask(unitTaskName, scObjectLog) - - # Add other states to log - scStateData = scObject.scStateOutMsg.recorder() - prescribedRotStateData = platform.prescribedRotationOutMsg.recorder() - unitTestSim.AddModelToTask(unitTaskName, scStateData) - unitTestSim.AddModelToTask(unitTaskName, prescribedRotStateData) - - # Initialize the simulation - unitTestSim.InitializeSimulation() - - # Set the simulation time - simTime = np.sqrt(((0.5 * np.abs(theta_Ref - thetaInit)) * 8) / accelMax) + 3 * testIncrement - unitTestSim.ConfigureStopTime(macros.sec2nano(simTime)) - - # Begin the simulation - unitTestSim.ExecuteSimulation() - - # Extract the logged data - orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) - orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) - rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) - rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) - omega_BN_B = scStateData.omega_BN_B - r_BN_N = scStateData.r_BN_N - sigma_BN = scStateData.sigma_BN - omega_PM_P= prescribedRotStateData.omega_PM_P - omegaPrime_PM_P= prescribedRotStateData.omegaPrime_PM_P - sigma_PM = prescribedRotStateData.sigma_PM - timespan = prescribedRotStateData.times() - thetaDot_Final = np.linalg.norm(omega_PM_P[-1, :]) - sigma_PM_Pinal = sigma_PM[-1, :] - theta_PM_Pinal = 4 * np.arctan(np.linalg.norm(sigma_PM_Pinal)) - - # Setup the conservation quantities - initialOrbAngMom_N = [[orbAngMom_N[0, 1], orbAngMom_N[0, 2], orbAngMom_N[0, 3]]] - finalOrbAngMom = [orbAngMom_N[-1]] - initialRotAngMom_N = [[rotAngMom_N[0, 1], rotAngMom_N[0, 2], rotAngMom_N[0, 3]]] - finalRotAngMom = [rotAngMom_N[-1]] - initialOrbEnergy = [[orbEnergy[0, 1]]] - finalOrbEnergy = [orbEnergy[-1]] - - # Convert the logged sigma_PM MRPs to a scalar theta_PM array - n = len(timespan) - theta_PM = [] - for i in range(n): - theta_PM.append((180 / np.pi) * (4 * np.arctan(np.linalg.norm(sigma_PM[i, :])))) - - plt.close("all") - - # Plot theta_PM - theta_Ref_plotting = np.ones(len(timespan)) * theta_Ref - thetaInit_plotting = np.ones(len(timespan)) * thetaInit - plt.figure() - plt.clf() - plt.plot(timespan * macros.NANO2SEC, theta_PM, label=r'$\Phi$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * theta_Ref_plotting, '--', label=r'$\Phi_{Ref}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * thetaInit_plotting, '--', label=r'$\Phi_{0}$') - plt.title(r'$\Phi_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) - plt.ylabel('(deg)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='center right', prop={'size': 16}) - - # Plot omega_PM_P - plt.figure() - plt.clf() - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_PM_P[:, 0], label=r'$\omega_{1}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_PM_P[:, 1], label=r'$\omega_{2}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_PM_P[:, 2], label=r'$\omega_{3}$') - plt.title(r'${}^\mathcal{P} \omega_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) - plt.ylabel('(deg/s)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='upper right', prop={'size': 16}) - - # Plotting omegaPrime_PM_P - plt.figure() - plt.clf() - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omegaPrime_PM_P[:, 0], label='1') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omegaPrime_PM_P[:, 1], label='2') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omegaPrime_PM_P[:, 2], label='3') - plt.title(r'${}^\mathcal{P} \omega Prime_{\mathcal{P}/\mathcal{M}}$ Profiled Angular Acceleration', fontsize=14) - plt.ylabel(r'(deg/$s^2$)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='upper right', prop={'size': 16}) - - # Plot r_BN_N - plt.figure() - plt.clf() - plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 0], label=r'$r_{1}$') - plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 1], label=r'$r_{2}$') - plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 2], label=r'$r_{3}$') - plt.title(r'${}^\mathcal{N} r_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial Trajectory', fontsize=14) - plt.ylabel('(m)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='center left', prop={'size': 16}) - - # Plot sigma_BN - plt.figure() - plt.clf() - plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 0], label=r'$\sigma_{1}$') - plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 1], label=r'$\sigma_{2}$') - plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 2], label=r'$\sigma_{3}$') - plt.title(r'$\sigma_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial MRP Attitude', fontsize=14) - plt.ylabel('', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='center left', prop={'size': 16}) - - # Plot omega_BN_B - plt.figure() - plt.clf() - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 0], label=r'$\omega_{1}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 1], label=r'$\omega_{2}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 2], label=r'$\omega_{3}$') - plt.title(r'Spacecraft Hub Angular Velocity ${}^\mathcal{B} \omega_{\mathcal{B}/\mathcal{N}}$', fontsize=14) - plt.xlabel('Time (s)', fontsize=16) - plt.ylabel('(deg/s)', fontsize=16) - plt.legend(loc='lower right', prop={'size': 16}) - - # Plotting: Conservation quantities - plt.figure() - plt.clf() - plt.plot(orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], - orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], - orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3]) - plt.title('Orbital Angular Momentum Relative Difference', fontsize=14) - plt.ylabel('(Nms)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - - plt.figure() - plt.clf() - plt.plot(orbEnergy[:, 0] * macros.NANO2SEC, (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1]) - plt.title('Orbital Energy Relative Difference', fontsize=14) - plt.ylabel('Energy (J)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - - plt.figure() - plt.clf() - plt.plot(rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]), - rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]), - rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 3] - rotAngMom_N[0, 3])) - plt.title('Rotational Angular Momentum Difference', fontsize=14) - plt.ylabel('(Nms)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - - plt.figure() - plt.clf() - plt.plot(rotEnergy[:, 0] * macros.NANO2SEC, (rotEnergy[:, 1] - rotEnergy[0, 1])) - plt.title('Total Energy Difference', fontsize=14) - plt.ylabel('Energy (J)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - - if show_plots: - plt.show() - plt.close("all") - - # Begin the test analysis - finalOrbAngMom = np.delete(finalOrbAngMom, 0, axis=1) # remove the time column - finalRotAngMom = np.delete(finalRotAngMom, 0, axis=1) # remove the time column - finalOrbEnergy = np.delete(finalOrbEnergy, 0, axis=1) # remove the time column - - for i in range(0, len(initialOrbAngMom_N)): - if not unitTestSupport.isArrayEqualRelative(finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy): - testFailCount += 1 - testMessages.append( - "FAILED: Prescribed Motion integrated test failed orbital angular momentum unit test") - - for i in range(0, len(initialRotAngMom_N)): - if not unitTestSupport.isArrayEqual(finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy): - testFailCount += 1 - testMessages.append( - "FAILED: Prescribed Motion integrated test failed rotational angular momentum unit test") - - for i in range(0, len(initialOrbEnergy)): - if not unitTestSupport.isArrayEqualRelative(finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy): - testFailCount += 1 - testMessages.append("FAILED: Prescribed Motion integrated test failed orbital energy unit test") - - # Check to ensure the initial angle rate converged to the reference angle rate - if not unitTestSupport.isDoubleEqual(thetaDot_Final, thetaDot_Ref, accuracy): - testFailCount += 1 - testMessages.append("FAILED: " + PrescribedRot1DOF.ModelTag + "thetaDot_Final and thetaDot_Ref do not match") - - # Check to ensure the initial angle converged to the reference angle - if not unitTestSupport.isDoubleEqual(theta_PM_Pinal, theta_Ref, accuracy): - testFailCount += 1 - testMessages.append("FAILED: " + PrescribedRot1DOF.ModelTag + "theta_PM_Pinal and theta_Ref do not match") - # testMessages.append("theta_PM_Pinal: " + str(theta_PM_Pinal) + " theta_Ref: " + str(theta_Ref)) - - if testFailCount == 0: - print("PASSED: " + "prescribedMotion and prescribedRot1DOF integrated test") - - else: - - # ** ** ** ** ** TRANSLATIONAL INTEGRATED TEST ** ** ** ** ** - - # Create an instance of the prescribedLinearTranslation module to be tested - PrescribedTrans = prescribedLinearTranslation.PrescribedLinearTranslation() - PrescribedTrans.ModelTag = "prescribedLinearTranslation" - - # Add the prescribedLinearTranslation test module to runtime call list - unitTestSim.AddModelToTask(unitTaskName, PrescribedTrans) - - # Initialize the prescribedLinearTranslation test module configuration data - accelMax = 0.005 # [m/s^2] - PrescribedTrans.setTransHat_M(transAxis_M) - PrescribedTrans.setTransAccelMax(accelMax) - PrescribedTrans.setTransPosInit(posInit) - - # Create the prescribedTrans input message - velRef = 0.0 # [m/s] - linearTranslationRigidBodyMessageData = messaging.LinearTranslationRigidBodyMsgPayload() - linearTranslationRigidBodyMessageData.rho = posRef - linearTranslationRigidBodyMessageData.rhoDot = velRef - linearTranslationRigidBodyMessage = messaging.LinearTranslationRigidBodyMsg().write(linearTranslationRigidBodyMessageData) - PrescribedTrans.linearTranslationRigidBodyInMsg.subscribeTo(linearTranslationRigidBodyMessage) - - platform.prescribedTranslationInMsg.subscribeTo(PrescribedTrans.prescribedTranslationOutMsg) - - # Add Earth gravity to the simulation - earthGravBody = gravityEffector.GravBodyData() - earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 - earthGravBody.isCentralBody = True - scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) - - # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"]) - unitTestSim.AddModelToTask(unitTaskName, scObjectLog) - - # Add other states to log - scStateData = scObject.scStateOutMsg.recorder() - prescribedTransStateData = platform.prescribedTranslationOutMsg.recorder() - unitTestSim.AddModelToTask(unitTaskName, scStateData) - unitTestSim.AddModelToTask(unitTaskName, prescribedTransStateData) - - # Initialize the simulation - unitTestSim.InitializeSimulation() - - # Set the simulation time - simTime = np.sqrt(((0.5 * np.abs(posRef - posInit)) * 8) / accelMax) + 3 * testIncrement - unitTestSim.ConfigureStopTime(macros.sec2nano(simTime)) - - # Begin the simulation - unitTestSim.ExecuteSimulation() - - # Extract the logged data - orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) - orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) - rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) - rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) - r_BN_N = scStateData.r_BN_N - sigma_BN = scStateData.sigma_BN - omega_BN_B = scStateData.omega_BN_B - r_PM_M = prescribedTransStateData.r_PM_M - rPrime_PM_M = prescribedTransStateData.rPrime_PM_M - rPrimePrime_PM_M = prescribedTransStateData.rPrimePrime_PM_M - timespan = prescribedTransStateData.times() - r_PM_M_Final = r_PM_M[-1, :] - rPrime_PM_M_Final = rPrime_PM_M[-1, :] - - # Setup the conservation quantities - initialOrbAngMom_N = [[orbAngMom_N[0, 1], orbAngMom_N[0, 2], orbAngMom_N[0, 3]]] - finalOrbAngMom = [orbAngMom_N[-1]] - initialRotAngMom_N = [[rotAngMom_N[0, 1], rotAngMom_N[0, 2], rotAngMom_N[0, 3]]] - finalRotAngMom = [rotAngMom_N[-1]] - initialOrbEnergy = [[orbEnergy[0, 1]]] - finalOrbEnergy = [orbEnergy[-1]] - initialRotEnergy = [[rotEnergy[0, 1]]] - finalRotEnergy = [rotEnergy[-1]] - - # Plot r_PM_P - r_PM_M_Ref = posRef * transAxis_M - r_PM_M_1_Ref = np.ones(len(timespan)) * r_PM_M_Ref[0] - r_PM_M_2_Ref = np.ones(len(timespan)) * r_PM_M_Ref[1] - r_PM_M_3_Ref = np.ones(len(timespan)) * r_PM_M_Ref[2] - - plt.figure() - plt.clf() - plt.plot(timespan * macros.NANO2SEC, r_PM_M[:, 0], label=r'$r_{1}$') - plt.plot(timespan * macros.NANO2SEC, r_PM_M[:, 1], label=r'$r_{2}$') - plt.plot(timespan * macros.NANO2SEC, r_PM_M[:, 2], label=r'$r_{3}$') - plt.plot(timespan * macros.NANO2SEC, r_PM_M_1_Ref, '--', label=r'$r_{1 Ref}$') - plt.plot(timespan * macros.NANO2SEC, r_PM_M_2_Ref, '--', label=r'$r_{2 Ref}$') - plt.plot(timespan * macros.NANO2SEC, r_PM_M_3_Ref, '--', label=r'$r_{3 Ref}$') - plt.title(r'${}^\mathcal{M} r_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) - plt.ylabel('(m)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='center left', prop={'size': 16}) - - # Plot rPrime_PM_P - plt.figure() - plt.clf() - plt.plot(timespan * macros.NANO2SEC, rPrime_PM_M[:, 0], label='1') - plt.plot(timespan * macros.NANO2SEC, rPrime_PM_M[:, 1], label='2') - plt.plot(timespan * macros.NANO2SEC, rPrime_PM_M[:, 2], label='3') - plt.title(r'${}^\mathcal{M} rPrime_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) - plt.ylabel('(m/s)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='upper left', prop={'size': 16}) - - # Plotting rPrimePrime_PM_M - plt.figure() - plt.clf() - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * rPrimePrime_PM_M[:, 0], label='1') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * rPrimePrime_PM_M[:, 1], label='2') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * rPrimePrime_PM_M[:, 2], label='3') - plt.title(r'${}^\mathcal{M} rPrimePrime_{\mathcal{P}/\mathcal{M}}$ Profiled Acceleration', fontsize=14) - plt.ylabel(r'(m/s$^2$)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='lower left', prop={'size': 16}) - - # Plot r_BN_N - plt.figure() - plt.clf() - plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 0], label=r'$r_{1}$') - plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 1], label=r'$r_{2}$') - plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 2], label=r'$r_{3}$') - plt.title(r'${}^\mathcal{N} r_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial Trajectory', fontsize=14) - plt.ylabel('(m)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='center left', prop={'size': 16}) - - # Plot sigma_BN - plt.figure() - plt.clf() - plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 0], label=r'$\sigma_{1}$') - plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 1], label=r'$\sigma_{2}$') - plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 2], label=r'$\sigma_{3}$') - plt.title(r'$\sigma_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial MRP Attitude', fontsize=14) - plt.ylabel('', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='lower left', prop={'size': 16}) - - # Plot omega_BN_B - plt.figure() - plt.clf() - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 0], label=r'$\omega_{1}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 1], label=r'$\omega_{2}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 2], label=r'$\omega_{3}$') - plt.title(r'Spacecraft Hub Angular Velocity ${}^\mathcal{B} \omega_{\mathcal{B}/\mathcal{N}}$', fontsize=14) - plt.ylabel('(deg/s)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='lower left', prop={'size': 16}) - - # Plotting: Conservation quantities - plt.figure() - plt.clf() - plt.plot(orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], - orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], - orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3]) - plt.title('Orbital Angular Momentum Relative Difference', fontsize=14) - plt.ylabel('(Nms)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - - plt.figure() - plt.clf() - plt.plot(orbEnergy[:, 0] * macros.NANO2SEC, (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1]) - plt.title('Orbital Energy Relative Difference', fontsize=14) - plt.ylabel('Energy (J)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - - plt.figure() - plt.clf() - plt.plot(rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]), - rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]), - rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 3] - rotAngMom_N[0, 3])) - plt.title('Rotational Angular Momentum Difference', fontsize=14) - plt.ylabel('(Nms)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - - plt.figure() - plt.clf() - plt.plot(rotEnergy[:, 0] * macros.NANO2SEC, (rotEnergy[:, 1] - rotEnergy[0, 1])) - plt.title('Total Energy Difference', fontsize=14) - plt.ylabel('Energy (J)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - - if show_plots: - plt.show() - plt.close("all") - - # Begin the test analysis - finalOrbAngMom = np.delete(finalOrbAngMom, 0, axis=1) # remove the time column - finalRotAngMom = np.delete(finalRotAngMom, 0, axis=1) # remove the time column - finalOrbEnergy = np.delete(finalOrbEnergy, 0, axis=1) # remove the time column - - for i in range(0, len(initialOrbAngMom_N)): - if not unitTestSupport.isArrayEqualRelative(finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy): - testFailCount += 1 - testMessages.append( - "FAILED: Prescribed Motion integrated test failed orbital angular momentum unit test") - - for i in range(0, len(initialRotAngMom_N)): - if not unitTestSupport.isArrayEqual(finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy): - testFailCount += 1 - testMessages.append( - "FAILED: Prescribed Motion integrated test failed rotational angular momentum unit test") - - for i in range(0, len(initialOrbEnergy)): - if not unitTestSupport.isArrayEqualRelative(finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy): - testFailCount += 1 - testMessages.append("FAILED: Prescribed Motion integrated test failed orbital energy unit test") - - # Check to ensure the initial velocity converged to the reference velocity - rPrime_PM_M_Ref = np.array([0.0, 0.0, 0.0]) - if not unitTestSupport.isArrayEqual(rPrime_PM_M_Final, rPrime_PM_M_Ref, 3, accuracy): - testFailCount += 1 - testMessages.append("FAILED: " + PrescribedTrans.ModelTag + "rPrime_PM_M_Final and rPrime_PM_M_Ref do not match") - testMessages.append("rPrime_PM_M_Final: " + str(rPrime_PM_M_Final) + " rPrime_PM_M_Ref: " + str(rPrime_PM_M_Ref)) - - # Check to ensure the initial position converged to the reference position - r_PM_M_Ref = np.array([posRef, 0.0, 0.0]) - if not unitTestSupport.isArrayEqual(r_PM_M_Final, r_PM_M_Ref, 3, accuracy): - testFailCount += 1 - testMessages.append("FAILED: " + PrescribedTrans.ModelTag + "r_PM_M_Final and r_PM_M_Ref do not match") - testMessages.append("r_PM_M_Final: " + str(r_PM_M_Final) + " r_PM_M_Ref: " + str(r_PM_M_Ref)) - - if testFailCount == 0: - print("PASSED: " + "prescribedMotion and prescribedLinearTranslation integrated test") - - return [testFailCount, ''.join(testMessages)] + task_name = "unitTask" + process_name = "testProcess" + sim = SimulationBaseClass.SimBaseClass() + process_rate = macros.sec2nano(0.001) + process = sim.CreateNewProcess(process_name) + process.addTask(sim.CreateNewTask(task_name, process_rate)) + + # Hub mass properties + mass_hub = 800 # [kg] + length_hub = 1.0 # [m] + width_hub = 1.0 # [m] + depth_hub = 1.0 # [m] + I_hub_11 = (1 / 12) * mass_hub * (length_hub * length_hub + depth_hub * depth_hub) # [kg m^2] + I_hub_22 = (1 / 12) * mass_hub * (length_hub * length_hub + width_hub * width_hub) # [kg m^2] + I_hub_33 = (1 / 12) * mass_hub * (width_hub * width_hub + depth_hub * depth_hub) # [kg m^2] + + # Create the spacecraft + sc_object = spacecraft.Spacecraft() + sc_object.ModelTag = "scObject" + sc_object.hub.mHub = mass_hub + sc_object.hub.r_BcB_B = [0.0, 0.0, 0.0] # [m] + sc_object.hub.IHubPntBc_B = [[I_hub_11, 0.0, 0.0], + [0.0, I_hub_22, 0.0], + [0.0, 0.0, I_hub_33]] # [kg m^2] + sc_object.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] + sc_object.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] + sc_object.hub.omega_BN_BInit = [[0.01], [-0.01], [0.01]] + sc_object.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] + sim.AddModelToTask(task_name, sc_object) + + # Define the prescribed motion state effector properties + trans_axis_M = np.array([1.0, 0.0, 0.0]) + rot_axis_M = np.array([1.0, 0.0, 0.0]) + prv_P0M = theta_init * rot_axis_M + sigma_P0M = rbk.PRV2MRP(prv_P0M) + + mass_prescribed = 100 # [kg] + length_prescribed = 1.0 # [m] + width_prescribed = 1.0 # [m] + depth_prescribed = 1.0 # [m] + I_prescribed_11 = (1 / 12) * mass_prescribed * (length_prescribed + * length_prescribed + + depth_prescribed + * depth_prescribed) # [kg m^2] + I_prescribed_22 = (1 / 12) * mass_prescribed * (length_prescribed + * length_prescribed + + width_prescribed + * width_prescribed) # [kg m^2] + I_prescribed_33 = (1 / 12) * mass_prescribed * (width_prescribed + * width_prescribed + + depth_prescribed + * depth_prescribed) # [kg m^2] + I_prescribed_Pc_P = [[I_prescribed_11, 0.0, 0.0], + [0.0, I_prescribed_22, 0.0], + [0.0, 0.0, I_prescribed_33]] # [kg m^2] + + # Create the prescribed motion state effector + prescribed_body = prescribedMotionStateEffector.PrescribedMotionStateEffector() + prescribed_body.ModelTag = "prescribedMotion" + prescribed_body.setMass(mass_prescribed) + prescribed_body.setIPntPc_P(I_prescribed_Pc_P) + prescribed_body.setR_MB_B([0.5, 0.0, 0.0]) + prescribed_body.setR_PcP_P([0.5, 0.0, 0.0]) + prescribed_body.setR_PM_M([0.0, 0.0, 0.0]) + prescribed_body.setRPrime_PM_M(np.array([0.0, 0.0, 0.0])) + prescribed_body.setRPrimePrime_PM_M(np.array([0.0, 0.0, 0.0])) + prescribed_body.setOmega_PM_P(np.array([0.0, 0.0, 0.0])) + prescribed_body.setOmegaPrime_PM_P(np.array([0.0, 0.0, 0.0])) + prescribed_body.setSigma_PM(sigma_P0M) + prescribed_body.setSigma_MB([0.0, 0.0, 0.0]) + sc_object.addStateEffector(prescribed_body) + sim.AddModelToTask(task_name, prescribed_body) + + # Create the prescribed rotational motion profiler + ang_accel_max = 0.5 * macros.D2R # [rad/s^2] + one_dof_rotation_profiler = prescribedRotation1DOF.PrescribedRotation1DOF() + one_dof_rotation_profiler.ModelTag = "prescribedRotation1DOF" + one_dof_rotation_profiler.setRotHat_M(rot_axis_M) + one_dof_rotation_profiler.setThetaDDotMax(ang_accel_max) + one_dof_rotation_profiler.setThetaInit(theta_init) + one_dof_rotation_profiler.setCoastOptionBangDuration(1.0) + one_dof_rotation_profiler.setSmoothingDuration(1.0) + sim.AddModelToTask(task_name, one_dof_rotation_profiler) + + # Create the prescribed rotational motion reference message + prescribed_rotation_msg_data = messaging.HingedRigidBodyMsgPayload() + prescribed_rotation_msg_data.theta = theta_ref + prescribed_rotation_msg_data.thetaDot = 0.0 + prescribed_rotation_msg = messaging.HingedRigidBodyMsg().write(prescribed_rotation_msg_data) + one_dof_rotation_profiler.spinningBodyInMsg.subscribeTo(prescribed_rotation_msg) + prescribed_body.prescribedRotationInMsg.subscribeTo(one_dof_rotation_profiler.prescribedRotationOutMsg) + + # Create the prescribed translational motion profiler + trans_accel_max = 0.005 # [m/s^2] + one_dof_translation_profiler = prescribedLinearTranslation.PrescribedLinearTranslation() + one_dof_translation_profiler.ModelTag = "prescribedLinearTranslation" + one_dof_translation_profiler.setTransHat_M(trans_axis_M) + one_dof_translation_profiler.setTransAccelMax(trans_accel_max) + one_dof_translation_profiler.setTransPosInit(pos_init) + one_dof_translation_profiler.setCoastOptionBangDuration(1.0) + one_dof_translation_profiler.setSmoothingDuration(1.0) + sim.AddModelToTask(task_name, one_dof_translation_profiler) + + # Create the prescribed translational motion reference message + prescribed_translation_msg_data = messaging.LinearTranslationRigidBodyMsgPayload() + prescribed_translation_msg_data.rho = pos_ref + prescribed_translation_msg_data.rhoDot = 0.0 + prescribed_translation_msg = messaging.LinearTranslationRigidBodyMsg().write(prescribed_translation_msg_data) + one_dof_translation_profiler.linearTranslationRigidBodyInMsg.subscribeTo(prescribed_translation_msg) + prescribed_body.prescribedTranslationInMsg.subscribeTo(one_dof_translation_profiler.prescribedTranslationOutMsg) + + # Add Earth gravity to the simulation + earth_gravity = gravityEffector.GravBodyData() + earth_gravity.planetName = "earth_planet_data" + earth_gravity.mu = 0.3986004415E+15 + earth_gravity.isCentralBody = True + sc_object.gravField.gravBodies = spacecraft.GravBodyVector([earth_gravity]) + + # Set up data logging + conservation_data_log = sc_object.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + prescribed_rotation_data_log = prescribed_body.prescribedRotationOutMsg.recorder() + prescribed_translation_data_log = prescribed_body.prescribedTranslationOutMsg.recorder() + one_dof_rotation_profiler_data_log = one_dof_rotation_profiler.spinningBodyOutMsg.recorder() + sim.AddModelToTask(task_name, conservation_data_log) + sim.AddModelToTask(task_name, prescribed_rotation_data_log) + sim.AddModelToTask(task_name, prescribed_translation_data_log) + sim.AddModelToTask(task_name, one_dof_rotation_profiler_data_log) + + # Run the simulation + sim.InitializeSimulation() + sim_time = 30.0 # [s] + sim.ConfigureStopTime(macros.sec2nano(sim_time)) + sim.ExecuteSimulation() + + # Extract the logged data + orb_energy = conservation_data_log.totOrbEnergy + orb_ang_mom_n = conservation_data_log.totOrbAngMomPntN_N + rot_ang_mom_n = conservation_data_log.totRotAngMomPntC_N + rot_energy = conservation_data_log.totRotEnergy + timespan = prescribed_rotation_data_log.times() * macros.NANO2SEC # [s] + prescribed_theta = macros.R2D * one_dof_rotation_profiler_data_log.theta # [deg] + omega_pm_p = prescribed_rotation_data_log.omega_PM_P * macros.R2D # [deg/s] + omega_prime_pm_p = prescribed_rotation_data_log.omegaPrime_PM_P * macros.R2D # [deg/s^2] + r_pm_m = prescribed_translation_data_log.r_PM_M # [m] + r_prime_pm_m = prescribed_translation_data_log.rPrime_PM_M # [m/s] + r_prime_prime_pm_m = prescribed_translation_data_log.rPrimePrime_PM_M # [m/s^2] + + # Plot prescribed position and angle + theta_ref_plotting = np.ones(len(timespan)) * theta_ref * macros.R2D # [deg] + prescribed_rho_ref_plotting = np.ones(len(timespan)) * pos_ref # [m] + prescribed_rho = np.dot(r_pm_m, trans_axis_M) # [m] + start_index = 3 + + fig1, ax1 = plt.subplots() + ax1.plot(timespan[start_index:], prescribed_theta[start_index:], label=r"$\theta$", color="teal") + ax1.plot(timespan[start_index:], theta_ref_plotting[start_index:], "--", label=r"$\theta_{\text{ref}}$", color="teal") + ax1.tick_params(axis="y", labelcolor="teal") + ax1.set_xlabel("Time (s)", fontsize=16) + ax1.set_ylabel("Angle (deg)", color="teal", fontsize=16) + ax2 = ax1.twinx() + ax2.plot(timespan[start_index:], 100.0 * prescribed_rho[start_index:], label=r"$\rho$", color="darkviolet") + ax2.plot(timespan[start_index:], 100.0 * prescribed_rho_ref_plotting[start_index:], "--", label=r"$\rho_{\text{ref}}$", color="darkviolet") + ax2.set_ylabel("Displacement (cm)", color="darkviolet", fontsize=16) + ax2.tick_params(axis="y", labelcolor="darkviolet") + handles_ax1, labels_ax1 = ax1.get_legend_handles_labels() + handles_ax2, labels_ax2 = ax2.get_legend_handles_labels() + handles = handles_ax1 + handles_ax2 + labels = labels_ax1 + labels_ax2 + plt.legend(handles=handles, labels=labels, loc="center left", prop={"size": 16}) + plt.grid(True) + + # Plot prescribed velocities + prescribed_rho_dot = np.dot(r_prime_pm_m, trans_axis_M) # [m/s] + prescribed_theta_dot = np.dot(omega_pm_p, rot_axis_M) # [deg/s] + + fig2, ax1 = plt.subplots() + ax1.plot(timespan[start_index:], prescribed_theta_dot[start_index:], label=r"$\dot{\theta}$", color="teal") + ax1.tick_params(axis="y", labelcolor="teal") + ax1.set_xlabel("Time (s)", fontsize=16) + ax1.set_ylabel("Angle Rate (deg/s)", color="teal", fontsize=16) + ax2 = ax1.twinx() + ax2.plot(timespan[start_index:], 100.0 * prescribed_rho_dot[start_index:], label=r"$\dot{\rho}$", color="darkviolet") + ax2.set_ylabel("Displacement Rate (cm/s)", color="darkviolet", fontsize=16) + ax2.tick_params(axis="y", labelcolor="darkviolet") + handles_ax1, labels_ax1 = ax1.get_legend_handles_labels() + handles_ax2, labels_ax2 = ax2.get_legend_handles_labels() + handles = handles_ax1 + handles_ax2 + labels = labels_ax1 + labels_ax2 + plt.legend(handles=handles, labels=labels, loc="center", prop={"size": 16}) + plt.grid(True) + + # Plot prescribed accelerations + prescribed_rho_ddot = np.dot(r_prime_prime_pm_m, trans_axis_M) + prescribed_theta_ddot = np.dot(omega_prime_pm_p, rot_axis_M) # [deg/s^2] + + fig3, ax1 = plt.subplots() + ax1.plot(timespan[start_index:], prescribed_theta_ddot[start_index:], label=r"$\ddot{\theta}$", color="teal") + ax1.tick_params(axis="y", labelcolor="teal") + ax1.set_xlabel("Time (s)", fontsize=16) + ax1.set_ylabel("Angular Acceleration (deg/$s^2$)", color="teal", fontsize=16) + ax2 = ax1.twinx() + ax2.plot(timespan[start_index:], 100.0 * prescribed_rho_ddot[start_index:], label=r"$\ddot{\rho}$", color="darkviolet") + ax2.set_ylabel("Linear Acceleration (cm/$s^2$)", color="darkviolet", fontsize=16) + ax2.tick_params(axis="y", labelcolor="darkviolet") + handles_ax1, labels_ax1 = ax1.get_legend_handles_labels() + handles_ax2, labels_ax2 = ax2.get_legend_handles_labels() + handles = handles_ax1 + handles_ax2 + labels = labels_ax1 + labels_ax2 + plt.legend(handles=handles, labels=labels, loc="upper right", prop={"size": 16}) + plt.grid(True) + + # Plot orbital angular momentum relative difference + plt.figure() + plt.clf() + plt.plot(timespan[start_index:], (orb_ang_mom_n[start_index:, 0] - orb_ang_mom_n[start_index, 0]) / orb_ang_mom_n[start_index, 0], color="teal", label=r'$\hat{n}_1$') + plt.plot(timespan[start_index:], (orb_ang_mom_n[start_index:, 1] - orb_ang_mom_n[start_index, 1]) / orb_ang_mom_n[start_index, 1], color="darkviolet", label=r'$\hat{n}_2$') + plt.plot(timespan[start_index:], (orb_ang_mom_n[start_index:, 2] - orb_ang_mom_n[start_index, 2]) / orb_ang_mom_n[start_index, 2], color="blue", label=r'$\hat{n}_3$') + plt.title('Orbital Angular Momentum Relative Difference', fontsize=16) + plt.ylabel('Relative Difference (Nms)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc='lower right', prop={'size': 16}) + plt.grid(True) + + # Plot orbital energy relative difference + plt.figure() + plt.clf() + plt.plot(timespan[start_index:], (orb_energy[start_index:] - orb_energy[start_index]) / orb_energy[start_index], color="teal") + plt.title('Orbital Energy Relative Difference', fontsize=16) + plt.ylabel('Relative Difference (J)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.grid(True) + + # Plot sc angular momentum relative difference + plt.figure() + plt.clf() + plt.plot(timespan[start_index:], (rot_ang_mom_n[start_index:, 0] - rot_ang_mom_n[start_index, 0]) / rot_ang_mom_n[start_index, 0], color="teal", label=r'$\hat{n}_1$') + plt.plot(timespan[start_index:], (rot_ang_mom_n[start_index:, 1] - rot_ang_mom_n[start_index, 1]) / rot_ang_mom_n[start_index, 1], color="darkviolet", label=r'$\hat{n}_2$') + plt.plot(timespan[start_index:], (rot_ang_mom_n[start_index:, 2] - rot_ang_mom_n[start_index, 2]) / rot_ang_mom_n[start_index, 2], color="blue", label=r'$\hat{n}_3$') + plt.title('Rotational Angular Momentum Relative Difference', fontsize=16) + plt.ylabel('Relative Difference (Nms)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc='upper right', prop={'size': 16}) + plt.grid(True) + + # Plot sc energy difference + plt.figure() + plt.clf() + plt.plot(timespan[start_index:], (rot_energy[start_index:] - rot_energy[start_index]), color="teal") + plt.title('Rotational Energy Difference', fontsize=16) + plt.ylabel('Difference (J)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.grid(True) + + if show_plots: + plt.show() + plt.close("all") + + # Check conservation of orbital angular momentum, energy, and sc rotational angular momentum + np.testing.assert_allclose(orb_ang_mom_n[start_index], orb_ang_mom_n[-1], rtol=1e-6, verbose=True) + np.testing.assert_allclose(orb_energy[start_index], orb_energy[-1], rtol=1e-6, verbose=True) + np.testing.assert_allclose(rot_ang_mom_n[start_index], rot_ang_mom_n[-1], rtol=1e-6, verbose=True) + -# -# This statement below ensures that the unitTestScript can be run as a -# stand-along python script -# if __name__ == "__main__": - PrescribedMotionTestFunction( - True, # show_plots - False, # rotTest, (True: prescribedRot1DOF integrated test, - # False: prescribedTrans integrated test) - 0.0, # thetaInit [rad] - np.pi / 12, # theta_Ref [rad] - 0.0, # posInit [m] - 0.5, # posRef [m] - 1e-8 # accuracy - ) + test_prescribed_motion_state_effector( + True, # show_plots + 0.0 * macros.D2R, # theta_init [rad] + 10.0 * macros.D2R, # theta_ref [rad] + 0.0, # pos_init [m] + 0.1, # pos_ref [m] + ) diff --git a/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffectorBranching.py b/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffectorBranching.py new file mode 100644 index 0000000000..103a5747e2 --- /dev/null +++ b/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffectorBranching.py @@ -0,0 +1,777 @@ +# ISC License +# +# Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado at Boulder +# +# Permission to use, copy, modify, and/or distribute this software for any +# purpose with or without fee is hereby granted, provided that the above +# copyright notice and this permission notice appear in all copies. +# +# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + +import inspect +import matplotlib +import matplotlib.pyplot as plt +import numpy as np +import os + +filename = inspect.getframeinfo(inspect.currentframe()).filename +path = os.path.dirname(os.path.abspath(filename)) +splitPath = path.split('simulation') + +from Basilisk.utilities import SimulationBaseClass, unitTestSupport, macros +from Basilisk.simulation import spacecraft +from Basilisk.simulation import gravityEffector +from Basilisk.simulation import prescribedMotionStateEffector +from Basilisk.simulation import prescribedLinearTranslation +from Basilisk.simulation import prescribedRotation1DOF +from Basilisk.simulation import linearTranslationOneDOFStateEffector +from Basilisk.simulation import spinningBodyOneDOFStateEffector +from Basilisk.simulation import spinningBodyTwoDOFStateEffector +from Basilisk.architecture import messaging +from Basilisk.utilities import RigidBodyKinematics as rbk + +matplotlib.rc('xtick', labelsize=16) +matplotlib.rc('ytick', labelsize=16) + +# Prescribed motion parameters +prescribed_pos_init = 0.0 # [m] +prescribed_pos_ref = 0.1 # [m] +prescribed_trans_axis_M = np.array([1.0, 0.0, 0.0]) +r_P0M_M = prescribed_pos_init * prescribed_trans_axis_M +prescribed_trans_accel_max = 0.005 # [m/s^2] + +prescribed_theta_init = 0.0 * macros.D2R +prescribed_theta_ref = 10.0 * macros.D2R # [rad] +prescribed_rot_axis_M = np.array([1.0, 0.0, 0.0]) +prv_P0M = prescribed_theta_init * prescribed_rot_axis_M +sigma_P0M = rbk.PRV2MRP(prv_P0M) +prescribed_ang_accel_max = 0.5 * macros.D2R # [rad/s^2] + +sim_time = 15.0 # [s] + +def test_prescribed_branching_spinning_body_one_dof(show_plots): + r""" + **Verification Test Description** + + This unit test checks the branching capability of the prescribed motion module by connecting a + :ref:`spinningbodyOneDOFStateEffector` to the prescribed motion state effector. Both the + :ref:`prescribedRotation1DOF` and :ref:`prescribedLinearTranslation` modules are configured so that + the prescribed body simultaneously translates and rotates relative to the spacecraft hub. The spinning body + is set up to have free spring motion about its axis of rotation. + + The test checks that the conservation quantities of the spacecraft orbital energy, orbital angular momentum, + and rotational angular momentum remain constant over the duration of the simulation. + + **Description of Variables Being Tested** + + The test checks that the conservation quantities of the spacecraft orbital energy, orbital angular momentum, + and rotational angular momentum remain constant over the duration of the simulation. + """ + + task_name = "unitTask" + process_name = "testProcess" + sim = SimulationBaseClass.SimBaseClass() + process_rate = macros.sec2nano(0.001) + process = sim.CreateNewProcess(process_name) + process.addTask(sim.CreateNewTask(task_name, process_rate)) + + # Create the spacecraft + sc_object = create_spacecraft_hub() + sim.AddModelToTask(task_name, sc_object) + + # Create prescribed motion object + prescribed_platform = create_prescribed_body(r_P0M_M, sigma_P0M) + sim.AddModelToTask(task_name, prescribed_platform) + sc_object.addStateEffector(prescribed_platform) + + # Create rotational motion profiler + one_dof_rotation_profiler = prescribedRotation1DOF.PrescribedRotation1DOF() + one_dof_rotation_profiler.ModelTag = "prescribedRotation1DOF" + one_dof_rotation_profiler.setRotHat_M(prescribed_rot_axis_M) + one_dof_rotation_profiler.setThetaDDotMax(prescribed_ang_accel_max) + one_dof_rotation_profiler.setThetaInit(prescribed_theta_init) + one_dof_rotation_profiler.setCoastOptionBangDuration(1.0) + one_dof_rotation_profiler.setSmoothingDuration(1.0) + sim.AddModelToTask(task_name, one_dof_rotation_profiler) + + # Create the rotational motion reference message + prescribed_rotation_msg_data = messaging.HingedRigidBodyMsgPayload() + prescribed_rotation_msg_data.theta = prescribed_theta_ref + prescribed_rotation_msg_data.thetaDot = 0.0 + prescribed_rotation_msg = messaging.HingedRigidBodyMsg().write(prescribed_rotation_msg_data) + one_dof_rotation_profiler.spinningBodyInMsg.subscribeTo(prescribed_rotation_msg) + prescribed_platform.prescribedRotationInMsg.subscribeTo(one_dof_rotation_profiler.prescribedRotationOutMsg) + + # Create translational motion profiler + one_dof_translation_profiler = prescribedLinearTranslation.PrescribedLinearTranslation() + one_dof_translation_profiler.ModelTag = "prescribedLinearTranslation" + one_dof_translation_profiler.setTransHat_M(prescribed_trans_axis_M) + one_dof_translation_profiler.setTransAccelMax(prescribed_trans_accel_max) + one_dof_translation_profiler.setTransPosInit(prescribed_pos_init) + one_dof_translation_profiler.setCoastOptionBangDuration(1.0) + one_dof_translation_profiler.setSmoothingDuration(1.0) + sim.AddModelToTask(task_name, one_dof_translation_profiler) + + # Create the translational motion reference message + prescribed_translation_msg_data = messaging.LinearTranslationRigidBodyMsgPayload() + prescribed_translation_msg_data.rho = prescribed_pos_ref + prescribed_translation_msg_data.rhoDot = 0.0 + prescribed_translation_msg = messaging.LinearTranslationRigidBodyMsg().write(prescribed_translation_msg_data) + one_dof_translation_profiler.linearTranslationRigidBodyInMsg.subscribeTo(prescribed_translation_msg) + prescribed_platform.prescribedTranslationInMsg.subscribeTo(one_dof_translation_profiler.prescribedTranslationOutMsg) + + # Create the spinning body + spinning_body_one_dof = spinningBodyOneDOFStateEffector.SpinningBodyOneDOFStateEffector() + spinning_body_one_dof.ModelTag = "spinningBodyOneDOF" + spinning_body_one_dof.mass = 50.0 + spinning_body_one_dof.IPntSc_S = [[50.0, 0.0, 0.0], [0.0, 30.0, 0.0], [0.0, 0.0, 40.0]] + spinning_body_one_dof.dcm_S0B = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] + spinning_body_one_dof.r_ScS_S = [[0.0], [0.0], [0.0]] + spinning_body_one_dof.r_SB_B = [[1.5], [0.0], [0.0]] + spinning_body_one_dof.sHat_S = [[1], [0], [0]] + spinning_body_one_dof.thetaInit = 10.0 * macros.D2R + spinning_body_one_dof.thetaDotInit = 0.0 * macros.D2R + spinning_body_one_dof.k = 100.0 + sim.AddModelToTask(task_name, spinning_body_one_dof) + + # Attach the spinning body to the prescribed motion prescribed_platform + prescribed_platform.addStateEffector(spinning_body_one_dof) + + # Add Earth gravity to the simulation + earth_gravity = gravityEffector.GravBodyData() + earth_gravity.planetName = "earth_planet_data" + earth_gravity.mu = 0.3986004415E+15 # meters! + earth_gravity.isCentralBody = True + sc_object.gravField.gravBodies = spacecraft.GravBodyVector([earth_gravity]) + + # Set up data logging + conservation_data_log = sc_object.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + prescribed_rotation_data_log = prescribed_platform.prescribedRotationOutMsg.recorder() + prescribed_translation_data_log = prescribed_platform.prescribedTranslationOutMsg.recorder() + one_dof_rotation_profiler_data_log = one_dof_rotation_profiler.spinningBodyOutMsg.recorder() + spinning_body_theta_data_log = spinning_body_one_dof.spinningBodyOutMsg.recorder() + sim.AddModelToTask(task_name, conservation_data_log) + sim.AddModelToTask(task_name, prescribed_rotation_data_log) + sim.AddModelToTask(task_name, prescribed_translation_data_log) + sim.AddModelToTask(task_name, one_dof_rotation_profiler_data_log) + sim.AddModelToTask(task_name, spinning_body_theta_data_log) + + # Run the simulation + sim.InitializeSimulation() + sim.ConfigureStopTime(macros.sec2nano(sim_time)) + sim.ExecuteSimulation() + + # Extract the logged variables + orb_energy = conservation_data_log.totOrbEnergy + orb_ang_mom_n = conservation_data_log.totOrbAngMomPntN_N + rot_ang_mom_n = conservation_data_log.totRotAngMomPntC_N + rot_energy = conservation_data_log.totRotEnergy + timespan = prescribed_rotation_data_log.times() * macros.NANO2SEC # [s] + prescribed_theta = macros.R2D * one_dof_rotation_profiler_data_log.theta # [deg] + omega_pm_p = prescribed_rotation_data_log.omega_PM_P * macros.R2D # [deg] + omega_prime_pm_p = prescribed_rotation_data_log.omegaPrime_PM_P * macros.R2D # [deg] + r_pm_m = prescribed_translation_data_log.r_PM_M + r_prime_pm_m = prescribed_translation_data_log.rPrime_PM_M + r_prime_prime_pm_m = prescribed_translation_data_log.rPrimePrime_PM_M + spinning_body_theta = macros.R2D * spinning_body_theta_data_log.theta + spinning_body_theta_dot = macros.R2D * spinning_body_theta_data_log.thetaDot + + # Plot results + plot_conservation(timespan, + orb_ang_mom_n, + orb_energy, + rot_ang_mom_n, + rot_energy) + plot_prescribed_motion(timespan, + prescribed_theta, + omega_pm_p, + omega_prime_pm_p, + r_pm_m, + r_prime_pm_m, + r_prime_prime_pm_m) + + # Plot spinning body angle + plt.figure() + plt.clf() + plt.plot(timespan, spinning_body_theta, label=r"$\theta$", color="teal") + plt.xlabel("Time (s)", fontsize=16) + plt.ylabel("Angle (deg)", fontsize=16) + plt.legend(loc="center right", prop={"size": 16}) + plt.grid(True) + + # Plot spinning body angle rate + plt.figure() + plt.clf() + plt.plot(timespan, spinning_body_theta_dot, label=r"$\dot{\theta}$", color="teal") + plt.xlabel("Time (s)", fontsize=16) + plt.ylabel("Angle Rate (deg/s)", fontsize=16) + plt.legend(loc="center right", prop={"size": 16}) + plt.grid(True) + + if show_plots: + plt.show() + plt.close("all") + + # Test check + unit_test_check_verification(orb_ang_mom_n, orb_energy, rot_ang_mom_n) + + +def test_prescribed_branching_spinning_body_two_dof(show_plots): + r""" + **Verification Test Description** + + This unit test checks the branching capability of the prescribed motion module by connecting a + :ref:`spinningbodyTwoDOFStateEffector` to the prescribed motion state effector. Both the + :ref:`prescribedRotation1DOF` and :ref:`prescribedLinearTranslation` modules are configured so that + the prescribed body simultaneously translates and rotates relative to the spacecraft hub. The spinning body + is set up to have free spring motion about its axes of rotation. + + The test checks that the conservation quantities of the spacecraft orbital energy, orbital angular momentum, + and rotational angular momentum remain constant over the duration of the simulation. + + **Description of Variables Being Tested** + + The test checks that the conservation quantities of the spacecraft orbital energy, orbital angular momentum, + and rotational angular momentum remain constant over the duration of the simulation. + """ + + task_name = "unitTask" + process_name = "testProcess" + sim = SimulationBaseClass.SimBaseClass() + process_rate = macros.sec2nano(0.001) + process = sim.CreateNewProcess(process_name) + process.addTask(sim.CreateNewTask(task_name, process_rate)) + + # Create the spacecraft + sc_object = create_spacecraft_hub() + sim.AddModelToTask(task_name, sc_object) + + # Create prescribed motion object + prescribed_platform = create_prescribed_body(r_P0M_M, sigma_P0M) + sim.AddModelToTask(task_name, prescribed_platform) + sc_object.addStateEffector(prescribed_platform) + + # Create rotational motion profiler + one_dof_rotation_profiler = prescribedRotation1DOF.PrescribedRotation1DOF() + one_dof_rotation_profiler.ModelTag = "prescribedRotation1DOF" + one_dof_rotation_profiler.setRotHat_M(prescribed_rot_axis_M) + one_dof_rotation_profiler.setThetaDDotMax(prescribed_ang_accel_max) + one_dof_rotation_profiler.setThetaInit(prescribed_theta_init) + one_dof_rotation_profiler.setCoastOptionBangDuration(1.0) + one_dof_rotation_profiler.setSmoothingDuration(1.0) + sim.AddModelToTask(task_name, one_dof_rotation_profiler) + + # Create the rotational motion reference message + prescribed_rotation_msg_data = messaging.HingedRigidBodyMsgPayload() + prescribed_rotation_msg_data.theta = prescribed_theta_ref + prescribed_rotation_msg_data.thetaDot = 0.0 + prescribed_rotation_msg = messaging.HingedRigidBodyMsg().write(prescribed_rotation_msg_data) + one_dof_rotation_profiler.spinningBodyInMsg.subscribeTo(prescribed_rotation_msg) + prescribed_platform.prescribedRotationInMsg.subscribeTo(one_dof_rotation_profiler.prescribedRotationOutMsg) + + # Create translational motion profiler + one_dof_translation_profiler = prescribedLinearTranslation.PrescribedLinearTranslation() + one_dof_translation_profiler.ModelTag = "prescribedLinearTranslation" + one_dof_translation_profiler.setTransHat_M(prescribed_trans_axis_M) + one_dof_translation_profiler.setTransAccelMax(prescribed_trans_accel_max) + one_dof_translation_profiler.setTransPosInit(prescribed_pos_init) + one_dof_translation_profiler.setCoastOptionBangDuration(1.0) + one_dof_translation_profiler.setSmoothingDuration(1.0) + sim.AddModelToTask(task_name, one_dof_translation_profiler) + + # Create the translational motion reference message + prescribed_translation_msg_data = messaging.LinearTranslationRigidBodyMsgPayload() + prescribed_translation_msg_data.rho = prescribed_pos_ref + prescribed_translation_msg_data.rhoDot = 0.0 + prescribed_translation_msg = messaging.LinearTranslationRigidBodyMsg().write(prescribed_translation_msg_data) + one_dof_translation_profiler.linearTranslationRigidBodyInMsg.subscribeTo(prescribed_translation_msg) + prescribed_platform.prescribedTranslationInMsg.subscribeTo(one_dof_translation_profiler.prescribedTranslationOutMsg) + + # Create the spinning body + spinning_body_two_dof = spinningBodyTwoDOFStateEffector.SpinningBodyTwoDOFStateEffector() + spinning_body_two_dof.ModelTag = "spinningBodyTwoDOF" + spinning_body_two_dof.mass1 = 100.0 + spinning_body_two_dof.mass2 = 50.0 + spinning_body_two_dof.IS1PntSc1_S1 = [[100.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]] + spinning_body_two_dof.IS2PntSc2_S2 = [[50.0, 0.0, 0.0], [0.0, 30.0, 0.0], [0.0, 0.0, 40.0]] + spinning_body_two_dof.dcm_S10B = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] + spinning_body_two_dof.dcm_S20S1 = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] + spinning_body_two_dof.r_Sc1S1_S1 = [[0.0], [0.0], [0.0]] + spinning_body_two_dof.r_Sc2S2_S2 = [[0.0], [0.0], [0.0]] + spinning_body_two_dof.r_S1B_B = [[1.5], [0.0], [0.0]] + spinning_body_two_dof.r_S2S1_S1 = [[1.0], [0.0], [0.0]] + spinning_body_two_dof.s1Hat_S1 = [[1], [0], [0]] + spinning_body_two_dof.s2Hat_S2 = [[1], [0], [0]] + spinning_body_two_dof.theta1Init = 0 * macros.D2R + spinning_body_two_dof.theta2Init = 5 * macros.D2R + spinning_body_two_dof.theta1DotInit = 2.0 * macros.D2R + spinning_body_two_dof.theta2DotInit = -1.0 * macros.D2R + spinning_body_two_dof.k1 = 1000.0 + spinning_body_two_dof.k2 = 500.0 + sim.AddModelToTask(task_name, spinning_body_two_dof) + + # Attach the spinning body to the prescribed motion prescribed_platform + prescribed_platform.addStateEffector(spinning_body_two_dof) + + # Add Earth gravity to the simulation + earth_gravity = gravityEffector.GravBodyData() + earth_gravity.planetName = "earth_planet_data" + earth_gravity.mu = 0.3986004415E+15 + earth_gravity.isCentralBody = True + sc_object.gravField.gravBodies = spacecraft.GravBodyVector([earth_gravity]) + + # Set up data logging + conservation_data_log = sc_object.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + prescribed_rotation_data_log = prescribed_platform.prescribedRotationOutMsg.recorder() + prescribed_translation_data_log = prescribed_platform.prescribedTranslationOutMsg.recorder() + one_dof_rotation_profiler_data_log = one_dof_rotation_profiler.spinningBodyOutMsg.recorder() + spinning_body_theta_1_data_log = spinning_body_two_dof.spinningBodyOutMsgs[0].recorder() + spinning_body_theta_2_data_log = spinning_body_two_dof.spinningBodyOutMsgs[1].recorder() + sim.AddModelToTask(task_name, conservation_data_log) + sim.AddModelToTask(task_name, prescribed_rotation_data_log) + sim.AddModelToTask(task_name, prescribed_translation_data_log) + sim.AddModelToTask(task_name, one_dof_rotation_profiler_data_log) + sim.AddModelToTask(task_name, spinning_body_theta_1_data_log) + sim.AddModelToTask(task_name, spinning_body_theta_2_data_log) + + # Run the simulation + sim.InitializeSimulation() + sim.ConfigureStopTime(macros.sec2nano(sim_time)) + sim.ExecuteSimulation() + + # Extract the logged variables + orb_energy = conservation_data_log.totOrbEnergy + orb_ang_mom_n = conservation_data_log.totOrbAngMomPntN_N + rot_ang_mom_n = conservation_data_log.totRotAngMomPntC_N + rot_energy = conservation_data_log.totRotEnergy + timespan = prescribed_rotation_data_log.times() * macros.NANO2SEC # [s] + prescribed_theta = macros.R2D * one_dof_rotation_profiler_data_log.theta # [deg] + omega_pm_p = prescribed_rotation_data_log.omega_PM_P * macros.R2D # [deg] + omega_prime_pm_p = prescribed_rotation_data_log.omegaPrime_PM_P * macros.R2D # [deg] + r_pm_m = prescribed_translation_data_log.r_PM_M + r_prime_pm_m = prescribed_translation_data_log.rPrime_PM_M + r_prime_prime_pm_m = prescribed_translation_data_log.rPrimePrime_PM_M + spinning_body_theta_1 = spinning_body_theta_1_data_log.theta + spinning_body_theta_1_dot = spinning_body_theta_1_data_log.thetaDot + spinning_body_theta_2 = spinning_body_theta_2_data_log.theta + spinning_body_theta_2_dot = spinning_body_theta_2_data_log.thetaDot + + # Plot results + plot_conservation(timespan, + orb_ang_mom_n, + orb_energy, + rot_ang_mom_n, + rot_energy) + plot_prescribed_motion(timespan, + prescribed_theta, + omega_pm_p, + omega_prime_pm_p, + r_pm_m, + r_prime_pm_m, + r_prime_prime_pm_m) + + # Plot spinning body theta 1 + plt.figure() + plt.clf() + plt.plot(timespan, spinning_body_theta_1, label=r"$\theta_1$", color="teal") + plt.xlabel("Time (s)", fontsize=16) + plt.ylabel("Angle (deg)", fontsize=16) + plt.legend(loc="center right", prop={"size": 16}) + plt.grid(True) + + # Plot spinning body theta dot 1 + plt.figure() + plt.clf() + plt.plot(timespan, spinning_body_theta_1_dot, label=r"$\dot{\theta}_1$", color="teal") + plt.xlabel("Time (s)", fontsize=16) + plt.ylabel("Angle Rate (deg/s)", fontsize=16) + plt.legend(loc="center right", prop={"size": 16}) + plt.grid(True) + + # Plot spinning body theta 2 + plt.figure() + plt.clf() + plt.plot(timespan, spinning_body_theta_2, label=r"$\theta_2$", color="teal") + plt.xlabel("Time (s)", fontsize=16) + plt.ylabel("Angle (deg)", fontsize=16) + plt.legend(loc="center right", prop={"size": 16}) + plt.grid(True) + + # Plot spinning body theta dot 2 + plt.figure() + plt.clf() + plt.plot(timespan, spinning_body_theta_2_dot, label=r"$\dot{\theta}_2$", color="teal") + plt.xlabel("Time (s)", fontsize=16) + plt.ylabel("Angle Rate (deg/s)", fontsize=16) + plt.legend(loc="center right", prop={"size": 16}) + plt.grid(True) + + if show_plots: + plt.show() + plt.close("all") + + # Test check + unit_test_check_verification(orb_ang_mom_n, orb_energy, rot_ang_mom_n) + + +def test_prescribed_branching_linear_translation_one_dof(show_plots): + r""" + **Verification Test Description** + + This unit test checks the branching capability of the prescribed motion module by connecting a + :ref:`linearTranslationOneDOFStateEffector` to the prescribed motion state effector. Both the + :ref:`prescribedRotation1DOF` and :ref:`prescribedLinearTranslation` modules are configured so that + the prescribed body simultaneously translates and rotates relative to the spacecraft hub. The translating body + is set up to have free spring motion about its axis of translation. + + The test checks that the conservation quantities of the spacecraft orbital energy, orbital angular momentum, + and rotational angular momentum remain constant over the duration of the simulation. + + **Description of Variables Being Tested** + + The test checks that the conservation quantities of the spacecraft orbital energy, orbital angular momentum, + and rotational angular momentum remain constant over the duration of the simulation. + """ + + task_name = "unitTask" + process_name = "testProcess" + sim = SimulationBaseClass.SimBaseClass() + process_rate = macros.sec2nano(0.001) + process = sim.CreateNewProcess(process_name) + process.addTask(sim.CreateNewTask(task_name, process_rate)) + + # Create the spacecraft + sc_object = create_spacecraft_hub() + sim.AddModelToTask(task_name, sc_object) + + # Create prescribed motion object + prescribed_platform = create_prescribed_body(r_P0M_M, sigma_P0M) + sim.AddModelToTask(task_name, prescribed_platform) + sc_object.addStateEffector(prescribed_platform) + + # Create the prescribed rotational motion profiler + one_dof_rotation_profiler = prescribedRotation1DOF.PrescribedRotation1DOF() + one_dof_rotation_profiler.ModelTag = "prescribedRotation1DOF" + one_dof_rotation_profiler.setRotHat_M(prescribed_rot_axis_M) + one_dof_rotation_profiler.setThetaDDotMax(prescribed_ang_accel_max) + one_dof_rotation_profiler.setThetaInit(prescribed_theta_init) + one_dof_rotation_profiler.setCoastOptionBangDuration(1.0) + one_dof_rotation_profiler.setSmoothingDuration(1.0) + sim.AddModelToTask(task_name, one_dof_rotation_profiler) + + # Create the prescribed rotational motion reference message + prescribed_rotation_msg_data = messaging.HingedRigidBodyMsgPayload() + prescribed_rotation_msg_data.theta = prescribed_theta_ref + prescribed_rotation_msg_data.thetaDot = 0.0 + prescribed_rotation_msg = messaging.HingedRigidBodyMsg().write(prescribed_rotation_msg_data) + one_dof_rotation_profiler.spinningBodyInMsg.subscribeTo(prescribed_rotation_msg) + prescribed_platform.prescribedRotationInMsg.subscribeTo(one_dof_rotation_profiler.prescribedRotationOutMsg) + + # Create the prescribed translational motion profiler + one_dof_translation_profiler = prescribedLinearTranslation.PrescribedLinearTranslation() + one_dof_translation_profiler.ModelTag = "prescribedLinearTranslation" + one_dof_translation_profiler.setTransHat_M(prescribed_trans_axis_M) + one_dof_translation_profiler.setTransAccelMax(prescribed_trans_accel_max) + one_dof_translation_profiler.setTransPosInit(prescribed_pos_init) + one_dof_translation_profiler.setCoastOptionBangDuration(1.0) + one_dof_translation_profiler.setSmoothingDuration(1.0) + sim.AddModelToTask(task_name, one_dof_translation_profiler) + + # Create the prescribed translational motion reference message + prescribed_translation_msg_data = messaging.LinearTranslationRigidBodyMsgPayload() + prescribed_translation_msg_data.rho = prescribed_pos_ref + prescribed_translation_msg_data.rhoDot = 0.0 + prescribed_translation_msg = messaging.LinearTranslationRigidBodyMsg().write(prescribed_translation_msg_data) + one_dof_translation_profiler.linearTranslationRigidBodyInMsg.subscribeTo(prescribed_translation_msg) + prescribed_platform.prescribedTranslationInMsg.subscribeTo(one_dof_translation_profiler.prescribedTranslationOutMsg) + + # Create the translating body + translating_body_one_dof = linearTranslationOneDOFStateEffector.LinearTranslationOneDOFStateEffector() + translating_body_one_dof.ModelTag = "translatingBody" + fHat_B = [[1.0], [0.0], [0.0]] + r_FcF_F = [[0.0], [0.0], [0.0]] + r_F0B_B = [[1.0], [0.0], [0.0]] + IPntFc_F = [[50.0, 0.0, 0.0], + [0.0, 80.0, 0.0], + [0.0, 0.0, 60.0]] + dcm_FB = [[1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0]] + translating_body_one_dof.setMass(20.0) + translating_body_one_dof.setK(100.0) + translating_body_one_dof.setRhoInit(1.0) + translating_body_one_dof.setRhoDotInit(0.0) + translating_body_one_dof.setFHat_B(fHat_B) + translating_body_one_dof.setR_FcF_F(r_FcF_F) + translating_body_one_dof.setR_F0B_B(r_F0B_B) + translating_body_one_dof.setIPntFc_F(IPntFc_F) + translating_body_one_dof.setDCM_FB(dcm_FB) + sim.AddModelToTask(task_name, translating_body_one_dof) + + # Attach the translating body to the prescribed motion prescribed_platform + prescribed_platform.addStateEffector(translating_body_one_dof) + + # Add Earth gravity to the simulation + earth_gravity = gravityEffector.GravBodyData() + earth_gravity.planetName = "earth_planet_data" + earth_gravity.mu = 0.3986004415E+15 # meters! + earth_gravity.isCentralBody = True + sc_object.gravField.gravBodies = spacecraft.GravBodyVector([earth_gravity]) + + # Set up data logging + conservation_data_log = sc_object.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + prescribed_rotation_data_log = prescribed_platform.prescribedRotationOutMsg.recorder() + prescribed_translation_data_log = prescribed_platform.prescribedTranslationOutMsg.recorder() + one_dof_rotation_profiler_data_log = one_dof_rotation_profiler.spinningBodyOutMsg.recorder() + translating_body_data_log = translating_body_one_dof.translatingBodyOutMsg.recorder() + sim.AddModelToTask(task_name, conservation_data_log) + sim.AddModelToTask(task_name, prescribed_rotation_data_log) + sim.AddModelToTask(task_name, prescribed_translation_data_log) + sim.AddModelToTask(task_name, one_dof_rotation_profiler_data_log) + sim.AddModelToTask(task_name, translating_body_data_log) + + # Run the simulation + sim.InitializeSimulation() + sim.ConfigureStopTime(macros.sec2nano(sim_time)) + sim.ExecuteSimulation() + + # Extract the logged variables + orb_energy = conservation_data_log.totOrbEnergy + orb_ang_mom_n = conservation_data_log.totOrbAngMomPntN_N + rot_ang_mom_n = conservation_data_log.totRotAngMomPntC_N + rot_energy = conservation_data_log.totRotEnergy + timespan = prescribed_rotation_data_log.times() * macros.NANO2SEC # [s] + prescribed_theta = macros.R2D * one_dof_rotation_profiler_data_log.theta # [deg] + omega_pm_p = prescribed_rotation_data_log.omega_PM_P * macros.R2D # [deg/s] + omega_prime_pm_p = prescribed_rotation_data_log.omegaPrime_PM_P * macros.R2D # [deg/s^2] + r_pm_m = prescribed_translation_data_log.r_PM_M # [m] + r_prime_pm_m = prescribed_translation_data_log.rPrime_PM_M # [m/s] + r_prime_prime_pm_m = prescribed_translation_data_log.rPrimePrime_PM_M # [m/s^2] + translating_body_rho = translating_body_data_log.rho # [m] + translating_body_rho_dot = translating_body_data_log.rhoDot # [m/s] + + # Plot results + plot_conservation(timespan, + orb_ang_mom_n, + orb_energy, + rot_ang_mom_n, + rot_energy) + plot_prescribed_motion(timespan, + prescribed_theta, + omega_pm_p, + omega_prime_pm_p, + r_pm_m, + r_prime_pm_m, + r_prime_prime_pm_m) + + # Plot translating body displacement + plt.figure() + plt.clf() + plt.plot(timespan, translating_body_rho, label=r"$\rho$", color="teal") + plt.xlabel("Time (s)", fontsize=16) + plt.ylabel("Displacement (m)", fontsize=16) + plt.legend(loc="upper right", prop={"size": 16}) + plt.grid(True) + + # Plot translating body displacement rate + plt.figure() + plt.clf() + plt.plot(timespan, translating_body_rho_dot, label=r"$\dot{\rho}$", color="teal") + plt.xlabel("Time (s)", fontsize=16) + plt.ylabel("Displacement Rate (m/s)", fontsize=16) + plt.legend(loc="upper right", prop={"size": 16}) + plt.grid(True) + + if show_plots: + plt.show() + plt.close("all") + + # Test check + unit_test_check_verification(orb_ang_mom_n, orb_energy, rot_ang_mom_n) + + +def create_spacecraft_hub(): + mass_hub = 800 # [kg] + length_hub = 1.0 # [m] + width_hub = 1.0 # [m] + depth_hub = 1.0 # [m] + I_hub_11 = (1 / 12) * mass_hub * (length_hub * length_hub + depth_hub * depth_hub) # [kg m^2] + I_hub_22 = (1 / 12) * mass_hub * (length_hub * length_hub + width_hub * width_hub) # [kg m^2] + I_hub_33 = (1 / 12) * mass_hub * (width_hub * width_hub + depth_hub * depth_hub) # [kg m^2] + + sc_object = spacecraft.Spacecraft() + sc_object.ModelTag = "scObject" + sc_object.hub.mHub = mass_hub # kg + sc_object.hub.r_BcB_B = [0.0, 0.0, 0.0] # [m] + sc_object.hub.IHubPntBc_B = [[I_hub_11, 0.0, 0.0], [0.0, I_hub_22, 0.0], [0.0, 0.0, I_hub_33]] # [kg m^2] (Hub approximated as a cube) + sc_object.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] + sc_object.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] + sc_object.hub.omega_BN_BInit = [[0.01], [-0.01], [0.01]] + sc_object.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] + + return sc_object + + +def create_prescribed_body(r_PM_M, sigma_PM): + mass_prescribed = 100 # [kg] + length_prescribed = 1.0 # [m] + width_prescribed = 1.0 # [m] + depth_prescribed = 1.0 # [m] + I_prescribed_11 = (1 / 12) * mass_prescribed * (length_prescribed * length_prescribed + depth_prescribed * depth_prescribed) # [kg m^2] + I_prescribed_22 = (1 / 12) * mass_prescribed * (length_prescribed * length_prescribed + width_prescribed * width_prescribed) # [kg m^2] + I_prescribed_33 = (1 / 12) * mass_prescribed * (width_prescribed * width_prescribed + depth_prescribed * depth_prescribed) # [kg m^2] + I_prescribed_Pc_P = [[I_prescribed_11, 0.0, 0.0], [0.0, I_prescribed_22, 0.0], [0.0, 0.0,I_prescribed_33]] # [kg m^2] (approximated as a cube) + + prescribed_platform = prescribedMotionStateEffector.PrescribedMotionStateEffector() + prescribed_platform.ModelTag = "prescribedMotion" + prescribed_platform.setMass(mass_prescribed) + prescribed_platform.setIPntPc_P(I_prescribed_Pc_P) + prescribed_platform.setR_MB_B([0.5, 0.0, 0.0]) + prescribed_platform.setR_PcP_P([0.5, 0.0, 0.0]) + prescribed_platform.setR_PM_M(r_PM_M) + prescribed_platform.setRPrime_PM_M(np.array([0.0, 0.0, 0.0])) + prescribed_platform.setRPrimePrime_PM_M(np.array([0.0, 0.0, 0.0])) + prescribed_platform.setOmega_PM_P(np.array([0.0, 0.0, 0.0])) + prescribed_platform.setOmegaPrime_PM_P(np.array([0.0, 0.0, 0.0])) + prescribed_platform.setSigma_PM(sigma_PM) + prescribed_platform.setSigma_MB([0.0, 0.0, 0.0]) + + return prescribed_platform + + +def plot_prescribed_motion(timespan, + prescribed_theta, + omega_pm_p, + omega_prime_pm_p, + r_pm_m, + r_prime_pm_m, + r_prime_prime_pm_m): + + # Plot prescribed position and angle + prescribed_theta_ref_plotting = np.ones(len(timespan)) * prescribed_theta_ref * macros.R2D # [deg] + prescribed_rho_ref_plotting = np.ones(len(timespan)) * prescribed_pos_ref # [m] + prescribed_rho = np.dot(r_pm_m, prescribed_trans_axis_M) # [m] + + fig1, ax1 = plt.subplots() + ax1.plot(timespan, prescribed_theta, label=r"$\theta$", color="teal") + ax1.plot(timespan, prescribed_theta_ref_plotting, "--", label=r"$\theta_{\text{ref}}$", color="teal") + ax1.tick_params(axis="y", labelcolor="teal") + ax1.set_xlabel("Time (s)", fontsize=16) + ax1.set_ylabel("Angle (deg)", color="teal", fontsize=16) + ax2 = ax1.twinx() + ax2.plot(timespan, 100.0 * prescribed_rho, label=r"$\rho$", color="darkviolet") + ax2.plot(timespan, 100.0 * prescribed_rho_ref_plotting, "--", label=r"$\rho_{\text{ref}}$", color="darkviolet") + ax2.set_ylabel("Displacement (cm)", color="darkviolet", fontsize=16) + ax2.tick_params(axis="y", labelcolor="darkviolet") + handles_ax1, labels_ax1 = ax1.get_legend_handles_labels() + handles_ax2, labels_ax2 = ax2.get_legend_handles_labels() + handles = handles_ax1 + handles_ax2 + labels = labels_ax1 + labels_ax2 + plt.legend(handles=handles, labels=labels, loc="center left", prop={"size": 16}) + plt.grid(True) + + # Plot prescribed velocities + prescribed_rho_dot = np.dot(r_prime_pm_m, prescribed_trans_axis_M) # [m/s] + prescribed_theta_dot = np.dot(omega_pm_p, prescribed_rot_axis_M) # [deg/s] + + fig2, ax1 = plt.subplots() + ax1.plot(timespan, prescribed_theta_dot, label=r"$\dot{\theta}$", color="teal") + ax1.tick_params(axis="y", labelcolor="teal") + ax1.set_xlabel("Time (s)", fontsize=16) + ax1.set_ylabel("Angle Rate (deg/s)", color="teal", fontsize=16) + ax2 = ax1.twinx() + ax2.plot(timespan, 100.0 * prescribed_rho_dot, label=r"$\dot{\rho}$", color="darkviolet") + ax2.set_ylabel("Displacement Rate (cm/s)", color="darkviolet", fontsize=16) + ax2.tick_params(axis="y", labelcolor="darkviolet") + handles_ax1, labels_ax1 = ax1.get_legend_handles_labels() + handles_ax2, labels_ax2 = ax2.get_legend_handles_labels() + handles = handles_ax1 + handles_ax2 + labels = labels_ax1 + labels_ax2 + plt.legend(handles=handles, labels=labels, loc="center", prop={"size": 16}) + plt.grid(True) + + # Plot prescribed accelerations + prescribed_rho_ddot = np.dot(r_prime_prime_pm_m, prescribed_trans_axis_M) + prescribed_theta_ddot = np.dot(omega_prime_pm_p, prescribed_rot_axis_M) # [deg/s^2] + + fig3, ax1 = plt.subplots() + ax1.plot(timespan, prescribed_theta_ddot, label=r"$\ddot{\theta}$", color="teal") + ax1.tick_params(axis="y", labelcolor="teal") + ax1.set_xlabel("Time (s)", fontsize=16) + ax1.set_ylabel("Angular Acceleration (deg/$s^2$)", color="teal", fontsize=16) + ax2 = ax1.twinx() + ax2.plot(timespan, 100.0 * prescribed_rho_ddot, label=r"$\ddot{\rho}$", color="darkviolet") + ax2.set_ylabel("Linear Acceleration (cm/$s^2$)", color="darkviolet", fontsize=16) + ax2.tick_params(axis="y", labelcolor="darkviolet") + handles_ax1, labels_ax1 = ax1.get_legend_handles_labels() + handles_ax2, labels_ax2 = ax2.get_legend_handles_labels() + handles = handles_ax1 + handles_ax2 + labels = labels_ax1 + labels_ax2 + plt.legend(handles=handles, labels=labels, loc="upper right", prop={"size": 16}) + plt.grid(True) + + +def plot_conservation(timespan, orb_ang_mom_n, orb_energy, rot_ang_mom_n, rot_energy): + + # Plot orbital angular momentum relative difference + plt.figure() + plt.clf() + plt.plot(timespan, (orb_ang_mom_n[:, 0] - orb_ang_mom_n[0, 0]) / orb_ang_mom_n[0, 0], color="teal", label=r'$\hat{n}_1$') + plt.plot(timespan, (orb_ang_mom_n[:, 1] - orb_ang_mom_n[0, 1]) / orb_ang_mom_n[0, 1], color="darkviolet", label=r'$\hat{n}_2$') + plt.plot(timespan, (orb_ang_mom_n[:, 2] - orb_ang_mom_n[0, 2]) / orb_ang_mom_n[0, 2], color="blue", label=r'$\hat{n}_3$') + plt.title('Orbital Angular Momentum Relative Difference', fontsize=16) + plt.ylabel('Relative Difference (Nms)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc='lower right', prop={'size': 16}) + plt.grid(True) + + # Plot orbital energy relative difference + plt.figure() + plt.clf() + plt.plot(timespan, (orb_energy - orb_energy[0]) / orb_energy[0], color="teal") + plt.title('Orbital Energy Relative Difference', fontsize=16) + plt.ylabel('Relative Difference (J)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.grid(True) + + # Plot sc angular momentum relative difference + plt.figure() + plt.clf() + plt.plot(timespan, (rot_ang_mom_n[:, 0] - rot_ang_mom_n[0, 0]) / rot_ang_mom_n[0, 0], color="teal", label=r'$\hat{n}_1$') + plt.plot(timespan, (rot_ang_mom_n[:, 1] - rot_ang_mom_n[0, 1]) / rot_ang_mom_n[0, 1], color="darkviolet", label=r'$\hat{n}_2$') + plt.plot(timespan, (rot_ang_mom_n[:, 2] - rot_ang_mom_n[0, 2]) / rot_ang_mom_n[0, 2], color="blue", label=r'$\hat{n}_3$') + plt.title('Rotational Angular Momentum Relative Difference', fontsize=16) + plt.ylabel('Relative Difference (Nms)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.legend(loc='upper right', prop={'size': 16}) + plt.grid(True) + + # Plot sc energy difference + plt.figure() + plt.clf() + plt.plot(timespan, (rot_energy - rot_energy[0]), color="teal") + plt.title('Rotational Energy Difference', fontsize=16) + plt.ylabel('Difference (J)', fontsize=16) + plt.xlabel('Time (s)', fontsize=16) + plt.grid(True) + + +def unit_test_check_verification(orb_ang_mom_n, orb_energy, rot_ang_mom_n): + + # Check conservation of orbital angular momentum, energy, and sc rotational angular momentum + np.testing.assert_allclose(orb_ang_mom_n[0], orb_ang_mom_n[-1], rtol=1e-6, verbose=True) + np.testing.assert_allclose(orb_energy[0], orb_energy[-1], rtol=1e-6, verbose=True) + np.testing.assert_allclose(rot_ang_mom_n[0], rot_ang_mom_n[-1], rtol=1e-6, verbose=True) + + +if __name__ == "__main__": + + test_prescribed_branching_spinning_body_one_dof(True) + test_prescribed_branching_spinning_body_two_dof(True) + test_prescribed_branching_linear_translation_one_dof(True) diff --git a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.cpp b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.cpp index e27c6903ba..f67197e9e6 100644 --- a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.cpp +++ b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.cpp @@ -45,8 +45,6 @@ PrescribedMotionStateEffector::PrescribedMotionStateEffector() this->IPntPc_P.setIdentity(); this->r_MB_B.setZero(); this->r_PcP_P.setZero(); - this->omega_MB_B.setZero(); - this->omegaPrime_MB_B.setZero(); this->sigma_MB.setIdentity(); this->currentSimTimeSec = 0.0; @@ -55,8 +53,20 @@ PrescribedMotionStateEffector::PrescribedMotionStateEffector() this->rPrimeEpoch_PM_M.setZero(); this->omegaEpoch_PM_P.setZero(); - // Set the sigma_PM state name - this->nameOfsigma_PMState = "prescribedMotionsigma_PM" + std::to_string(this->effectorID); + this->spacecraftName = "prescribedObject"; + this->nameOfsigma_PMState = "prescribedObjectsigma_PM" + std::to_string(this->effectorID); + + // Set the property names + this->nameOfInertialPositionProperty = "prescribedObjectInertialPosition" + std::to_string(PrescribedMotionStateEffector::effectorID); + this->nameOfInertialVelocityProperty = "prescribedObjectInertialVelocity" + std::to_string(PrescribedMotionStateEffector::effectorID); + this->nameOfInertialAttitudeProperty = "prescribedObjectInertialAttitude" + std::to_string(PrescribedMotionStateEffector::effectorID); + this->nameOfInertialAngVelocityProperty = "prescribedObjectInertialAngVelocity" + std::to_string(PrescribedMotionStateEffector::effectorID); + this->nameOfPrescribedPositionProperty = "prescribedObjectPosition" + std::to_string(PrescribedMotionStateEffector::effectorID); + this->nameOfPrescribedVelocityProperty = "prescribedObjectVelocity" + std::to_string(PrescribedMotionStateEffector::effectorID); + this->nameOfPrescribedAccelerationProperty = "prescribedObjectAcceleration" + std::to_string(PrescribedMotionStateEffector::effectorID); + this->nameOfPrescribedAttitudeProperty = "prescribedObjectAttitude" + std::to_string(PrescribedMotionStateEffector::effectorID); + this->nameOfPrescribedAngVelocityProperty = "prescribedObjectAngVelocity" + std::to_string(PrescribedMotionStateEffector::effectorID); + this->nameOfPrescribedAngAccelerationProperty = "prescribedObjectAngAcceleration" + std::to_string(PrescribedMotionStateEffector::effectorID); PrescribedMotionStateEffector::effectorID++; } @@ -113,8 +123,8 @@ void PrescribedMotionStateEffector::writeOutputStateMessages(uint64_t currentClo // Note that the configLogMsg B frame represents the effector body frame (frame P) eigenVector3d2CArray(this->r_PcN_N, configLogMsg.r_BN_N); eigenVector3d2CArray(this->v_PcN_N, configLogMsg.v_BN_N); - eigenVector3d2CArray(this->sigma_PN, configLogMsg.sigma_BN); - eigenVector3d2CArray(this->omega_PN_P, configLogMsg.omega_BN_B); + eigenMatrixXd2CArray(*this->sigma_PN, configLogMsg.sigma_BN); + eigenMatrixXd2CArray(*this->omega_PN_P, configLogMsg.omega_BN_B); this->prescribedMotionConfigLogOutMsg.write(&configLogMsg, this->moduleID, currentClock); } } @@ -128,6 +138,14 @@ void PrescribedMotionStateEffector::linkInStates(DynParamManager& statesIn) // Get access to the hub states needed for dynamic coupling this->inertialPositionProperty = statesIn.getPropertyReference(this->propName_inertialPosition); this->inertialVelocityProperty = statesIn.getPropertyReference(this->propName_inertialVelocity); + + // Loop through attached stateEffectors to link in their states + std::vector::iterator stateIt; + for(stateIt = this->stateEffectors.begin(); stateIt != this->stateEffectors.end(); stateIt++) + { + (*stateIt)->linkInStates(statesIn); + (*stateIt)->linkInPrescribedMotionProperties(statesIn); + } } /*! This method allows the state effector to register its states with the dynamic parameter manager. @@ -136,13 +154,43 @@ void PrescribedMotionStateEffector::linkInStates(DynParamManager& statesIn) */ void PrescribedMotionStateEffector::registerStates(DynParamManager& states) { - this->sigma_PMState = states.registerState(3, 1, this->nameOfsigma_PMState); - Eigen::Vector3d sigma_PM_loc = eigenMRPd2Vector3d(this->sigma_PM); - Eigen::Vector3d sigma_PMInitMatrix; - sigma_PMInitMatrix(0) = sigma_PM_loc[0]; - sigma_PMInitMatrix(1) = sigma_PM_loc[1]; - sigma_PMInitMatrix(2) = sigma_PM_loc[2]; - this->sigma_PMState->setState(sigma_PMInitMatrix); + this->sigma_PMState = states.registerState(3, 1, this->nameOfsigma_PMState); + Eigen::Vector3d sigma_PM_loc = eigenMRPd2Vector3d(this->sigma_PM); + Eigen::Vector3d sigma_PMInitMatrix; + sigma_PMInitMatrix(0) = sigma_PM_loc[0]; + sigma_PMInitMatrix(1) = sigma_PM_loc[1]; + sigma_PMInitMatrix(2) = sigma_PM_loc[2]; + this->sigma_PMState->setState(sigma_PMInitMatrix); + + // Call method to register the prescribed motion properties + registerProperties(states); + + // Loop through attached stateEffectors to register their states + std::vector::iterator stateIt; + for(stateIt = this->stateEffectors.begin(); stateIt != this->stateEffectors.end(); stateIt++) + { + (*stateIt)->registerStates(states); + } +} + +/*! This method allows the state effector to register its properties with the dynamic parameter manager. + + @param states Pointer to give the state effector access the hub states +*/ +void PrescribedMotionStateEffector::registerProperties(DynParamManager& states) +{ + Eigen::Vector3d stateInit = Eigen::Vector3d::Zero(); + this->r_PN_N = states.createProperty(this->nameOfInertialPositionProperty, stateInit); + this->v_PN_N = states.createProperty(this->nameOfInertialVelocityProperty, stateInit); + this->sigma_PN = states.createProperty(this->nameOfInertialAttitudeProperty, stateInit); + this->omega_PN_P = states.createProperty(this->nameOfInertialAngVelocityProperty, stateInit); + + this->r_PB_B = states.createProperty(this->nameOfPrescribedPositionProperty, stateInit); + this->rPrime_PB_B = states.createProperty(this->nameOfPrescribedVelocityProperty, stateInit); + this->rPrimePrime_PB_B = states.createProperty(this->nameOfPrescribedAccelerationProperty, stateInit); + this->sigma_PB = states.createProperty(this->nameOfPrescribedAttitudeProperty, stateInit); + this->omega_PB_P = states.createProperty(this->nameOfPrescribedAngVelocityProperty, stateInit); + this->omegaPrime_PB_P = states.createProperty(this->nameOfPrescribedAngAccelerationProperty, stateInit); } /*! This method allows the state effector to provide its contributions to the mass props and mass prop rates of the @@ -170,26 +218,29 @@ void PrescribedMotionStateEffector::updateEffectorMassProps(double integTime) // Compute dcm_BP this->dcm_BP = this->dcm_BM * this->dcm_PM.transpose(); + *this->sigma_PB = eigenMRPd2Vector3d(eigenC2MRP(this->dcm_BP.transpose())); - // Compute omega_PB_B given the user inputs omega_MB_M and omega_PM_P + // Compute omega_PB_B this->omega_PM_B = this->dcm_BP * this->omega_PM_P; - this->omega_PB_B = this->omega_PM_B + this->omega_MB_B; + this->omega_PB_B = this->omega_PM_B; + *this->omega_PB_P = this->dcm_BP.transpose() * this->omega_PB_B; // Compute omegaPrime_PB_B given the user inputs this->omegaTilde_PB_B = eigenTilde(this->omega_PB_B); this->omegaPrime_PM_B = this->dcm_BP * this->omegaPrime_PM_P; - this->omegaPrime_PB_B = this->omegaPrime_PM_B + this->omegaTilde_PB_B * this->omega_PM_B; + this->omegaPrime_PB_B = this->omegaPrime_PM_B; + *this->omegaPrime_PB_P = this->omegaPrime_PM_P; // Convert the prescribed variables to the B frame this->r_PM_B = this->dcm_BM * this->r_PM_M; this->rPrime_PM_B = this->dcm_BM * this->rPrime_PM_M; this->rPrimePrime_PM_B = this->dcm_BM * this->rPrimePrime_PM_M; + *this->rPrimePrime_PB_B = this->rPrimePrime_PM_B; // Compute the effector's CoM with respect to point B - this->r_PB_B = this->r_PM_B + this->r_MB_B; + *this->r_PB_B = this->r_PM_B + this->r_MB_B; this->r_PcP_B = this->dcm_BP * this->r_PcP_P; - this->r_PcB_B = this->r_PcP_B + this->r_PB_B; - this->effProps.rEff_CB_B = this->r_PcB_B; + this->r_PcB_B = this->r_PcP_B + *this->r_PB_B; // Find the effector inertia about point B this->rTilde_PcB_B = eigenTilde(this->r_PcB_B); @@ -197,9 +248,15 @@ void PrescribedMotionStateEffector::updateEffectorMassProps(double integTime) this->effProps.IEffPntB_B = this->IPntPc_B - this->mass * this->rTilde_PcB_B * this->rTilde_PcB_B; // Find the B frame time derivative of r_PcB_B - this->omegaTilde_PB_B = eigenTilde(this->omega_PB_B); this->rPrime_PcB_B = this->omegaTilde_PB_B * this->r_PcP_B + this->rPrime_PM_B; - this->effProps.rEffPrime_CB_B = this->rPrime_PcB_B; + + if (this->stateEffectors.empty()) { + this->effProps.rEff_CB_B = this->r_PcB_B; + this->effProps.rEffPrime_CB_B = this->rPrime_PcB_B; + } else { + this->effProps.rEff_CB_B = this->mass * this->r_PcB_B; + this->effProps.rEffPrime_CB_B = this->mass * this->rPrime_PcB_B; + } // Find the B frame time derivative of IPntPc_B Eigen::Matrix3d rPrimeTilde_PcB_B = eigenTilde(this->rPrime_PcB_B); @@ -207,6 +264,51 @@ void PrescribedMotionStateEffector::updateEffectorMassProps(double integTime) - this->IPntPc_B * this->omegaTilde_PB_B + this->mass * (rPrimeTilde_PcB_B * this->rTilde_PcB_B.transpose() + this->rTilde_PcB_B * rPrimeTilde_PcB_B.transpose()); + + + // Loop through attached state effectors and compute their contributions + std::vector::iterator it; + for(it = this->stateEffectors.begin(); it != this->stateEffectors.end(); it++) { + (*it)->updateEffectorMassProps(integTime); + + this->effProps.mEff += (*it)->effProps.mEff; + this->effProps.mEffDot += (*it)->effProps.mEffDot; + + Eigen::Vector3d r_EcP_P = (*it)->effProps.rEff_CB_B; + Eigen::Vector3d r_EcP_B = this->dcm_BP * r_EcP_P; + Eigen::Vector3d r_EcB_B = r_EcP_B + *this->r_PB_B; + this->effProps.rEff_CB_B += (*it)->effProps.mEff * r_EcB_B; + + Eigen::Vector3d rPPrime_EcP_P = (*it)->effProps.rEffPrime_CB_B; + Eigen::Vector3d rPrime_EcP_B = this->dcm_BP * rPPrime_EcP_P + this->omegaTilde_PB_B * r_EcP_B; + *this->rPrime_PB_B = this->rPrime_PM_B; + Eigen::Vector3d rPrime_EcB_B = rPrime_EcP_B + *this->rPrime_PB_B; + this->effProps.rEffPrime_CB_B += (*it)->effProps.mEff * rPrime_EcB_B; + + Eigen::Matrix3d IEffPntP_P = (*it)->effProps.IEffPntB_B; + Eigen::Matrix3d IEffPntP_B = this->dcm_BP * IEffPntP_P * this->dcm_BP.transpose(); + Eigen::Matrix3d rTilde_EcB_B = eigenTilde(r_EcB_B); + Eigen::Matrix3d rTilde_EcP_B = eigenTilde(r_EcP_B); + this->effProps.IEffPntB_B += IEffPntP_B + (*it)->effProps.mEff * (rTilde_EcP_B * rTilde_EcP_B + - rTilde_EcB_B * rTilde_EcB_B); + + Eigen::Matrix3d rPrimeTilde_EcB_B = eigenTilde(rPrime_EcB_B); + Eigen::Matrix3d rPrimeTilde_EcP_B = eigenTilde(rPrime_EcP_B); + Eigen::Matrix3d IEffPPrimePntP_P = (*it)->effProps.IEffPrimePntB_B; + Eigen::Matrix3d IEffPPrimePntP_B = this->dcm_BP * IEffPPrimePntP_P * this->dcm_BP.transpose(); + Eigen::Matrix3d IEffPrimePntP_B = IEffPPrimePntP_B + this->omegaTilde_PB_B * IEffPntP_B + + IEffPntP_B * this->omegaTilde_PB_B.transpose(); + this->effProps.IEffPrimePntB_B += IEffPrimePntP_B - (*it)->effProps.mEff * rPrimeTilde_EcP_B * rTilde_EcP_B.transpose() + - (*it)->effProps.mEff * rTilde_EcP_B * rPrimeTilde_EcP_B.transpose() + + (*it)->effProps.mEff * rPrimeTilde_EcB_B * rTilde_EcB_B.transpose() + + (*it)->effProps.mEff * rTilde_EcB_B * rPrimeTilde_EcB_B.transpose(); + } + + // Divide by the total mass of the prescribed component plus attached effectors to finalize mass props + if (!this->stateEffectors.empty()) { + this->effProps.rEff_CB_B = this->effProps.rEff_CB_B / this->effProps.mEff; + this->effProps.rEffPrime_CB_B = this->effProps.rEffPrime_CB_B / this->effProps.mEff; + } } /*! This method allows the state effector to give its contributions to the matrices needed for the back-sub. @@ -225,26 +327,73 @@ void PrescribedMotionStateEffector::updateContributions(double integTime, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) { - // Compute dcm_BN + // Update sigma_BN and dcm_BN this->sigma_BN = sigma_BN; this->dcm_BN = (this->sigma_BN.toRotationMatrix()).transpose(); - // Define omega_BN_B + // Update omega_BN_B this->omega_BN_B = omega_BN_B; this->omegaTilde_BN_B = eigenTilde(this->omega_BN_B); - // Define omegaPrimeTilde_PB_B - Eigen::Matrix3d omegaPrimeTilde_PB_B = eigenTilde(this->omegaPrime_PB_B); - - // Compute rPrimePrime_PcB_B - this->rPrimePrime_PcB_B = (omegaPrimeTilde_PB_B + this->omegaTilde_PB_B * this->omegaTilde_PB_B) * this->r_PcP_B + this->rPrimePrime_PM_B; + // Update sigma_PN + Eigen::Matrix3d dcm_PN = this->dcm_BP.transpose() * this->dcm_BN; + *this->sigma_PN = eigenMRPd2Vector3d(eigenC2MRP(dcm_PN)); + + // Update omega_PN_P + *this->omega_PN_P = *this->omega_PB_P + this->dcm_BP.transpose() * this->omega_BN_B; + + Eigen::Matrix3d totMatrixA; + Eigen::Matrix3d totMatrixB; + Eigen::Matrix3d totMatrixC; + Eigen::Matrix3d totMatrixD; + Eigen::Vector3d totVecTrans; + Eigen::Vector3d totVecRot; + totMatrixA.setZero(); + totMatrixB.setZero(); + totMatrixC.setZero(); + totMatrixD.setZero(); + totVecTrans.setZero(); + totVecRot.setZero(); + + // Loop through attached state effectors and compute their contributions + std::vector::iterator it; + for(it = this->stateEffectors.begin(); it != this->stateEffectors.end(); it++) + { + backSubContr.matrixA.setZero(); + backSubContr.matrixB.setZero(); + backSubContr.matrixC.setZero(); + backSubContr.matrixD.setZero(); + backSubContr.vecTrans.setZero(); + backSubContr.vecRot.setZero(); + + (*it)->updateContributions(integTime, backSubContr, *this->sigma_PN, *this->omega_PN_P, g_N); + (*it)->addPrescribedMotionCouplingContributions(backSubContr); + + totMatrixA += this->dcm_BP * backSubContr.matrixA * this->dcm_BP.transpose(); + totMatrixB += this->dcm_BP * backSubContr.matrixB * this->dcm_BP.transpose(); + totMatrixC += this->dcm_BP * backSubContr.matrixC * this->dcm_BP.transpose(); + totMatrixD += this->dcm_BP * backSubContr.matrixD * this->dcm_BP.transpose(); + totVecTrans += this->dcm_BP * backSubContr.vecTrans; + totVecRot += this->dcm_BP * backSubContr.vecRot; + } - // Translation contributions - backSubContr.vecTrans = -this->mass * this->rPrimePrime_PcB_B; + backSubContr.matrixA = totMatrixA; + backSubContr.matrixB = totMatrixB; + backSubContr.matrixC = totMatrixC; + backSubContr.matrixD = totMatrixD; + backSubContr.vecTrans = totVecTrans; + backSubContr.vecRot = totVecRot; - // Rotation contributions - Eigen::Matrix3d IPrimePntPc_B = this->omegaTilde_PB_B * this->IPntPc_B - this->IPntPc_B * this->omegaTilde_PB_B; - backSubContr.vecRot = -(this->mass * this->rTilde_PcB_B * this->rPrimePrime_PcB_B) + // Prescribed motion translation contributions + Eigen::Matrix3d omegaPrimeTilde_PB_B = eigenTilde(this->omegaPrime_PB_B); + this->rPrimePrime_PcB_B = (omegaPrimeTilde_PB_B + this->omegaTilde_PB_B * this->omegaTilde_PB_B) * this->r_PcP_B + + this->rPrimePrime_PM_B; + backSubContr.vecTrans += -this->mass * this->rPrimePrime_PcB_B; + + // Prescribed motion rotation contributions + Eigen::Matrix3d IPrimePntPc_B; + IPrimePntPc_B = this->omegaTilde_PB_B * this->IPntPc_B - this->IPntPc_B * this->omegaTilde_PB_B; + backSubContr.vecRot += -(this->mass * this->rTilde_PcB_B * this->rPrimePrime_PcB_B) - (IPrimePntPc_B + this->omegaTilde_BN_B * this->IPntPc_B) * this->omega_PB_B - this->IPntPc_B * this->omegaPrime_PB_B - this->mass * this->omegaTilde_BN_B * rTilde_PcB_B * this->rPrime_PcB_B; @@ -267,6 +416,33 @@ void PrescribedMotionStateEffector::computeDerivatives(double integTime, Eigen::MRPd sigma_PM_loc; sigma_PM_loc = (Eigen::Vector3d)this->sigma_PMState->getState(); this->sigma_PMState->setDerivative(0.25*sigma_PM_loc.Bmat()*this->omega_PM_P); + + // Loop through attached state effectors for compute derivatives + if (!this->stateEffectors.empty()) { + // Update sigma_BN and dcm_BN + this->sigma_BN = sigma_BN; + this->dcm_BN = (this->sigma_BN.toRotationMatrix()).transpose(); + + // Compute dcm_FN and sigma_FN + Eigen::Matrix3d dcm_PN = this->dcm_BP.transpose() * this->dcm_BN; + *this->sigma_PN = eigenMRPd2Vector3d(eigenC2MRP(dcm_PN)); + + // Compute omegaDot_PN_P + Eigen::Vector3d omegaDot_PN_B = this->omegaPrime_PM_B + this->omegaTilde_BN_B * this->omega_PM_B + omegaDot_BN_B; + Eigen::Vector3d omegaDot_PN_P = this->dcm_BP.transpose() * omegaDot_PN_B; + + // Compute rDDot_PN_N + Eigen::Matrix3d omegaDotTilde_BN_B = eigenTilde(omegaDot_BN_B); + Eigen::Vector3d rDDot_PB_B = this->rPrimePrime_PM_B + 2 * this->omegaTilde_BN_B * this->rPrime_PM_B + + omegaDotTilde_BN_B * (*this->r_PB_B) + + this->omegaTilde_BN_B * this->omegaTilde_BN_B * (*this->r_PB_B); + Eigen::Vector3d rDDot_PN_N = this->dcm_BN.transpose() * rDDot_PB_B + rDDot_BN_N; + + std::vector::iterator it; + for(it = this->stateEffectors.begin(); it != this->stateEffectors.end(); it++) { + (*it)->computeDerivatives(integTime, rDDot_PN_N, omegaDot_PN_P, *this->sigma_PN); + } + } } /*! This method is for calculating the contributions of the effector to the energy and momentum of the spacecraft. @@ -282,19 +458,51 @@ void PrescribedMotionStateEffector::updateEnergyMomContributions(double integTim double & rotEnergyContr, Eigen::Vector3d omega_BN_B) { - // Update omega_BN_B and omega_PN_B + // Update omega_BN_B, omega_PN_B, and omega_PN_P this->omega_BN_B = omega_BN_B; this->omegaTilde_BN_B = eigenTilde(this->omega_BN_B); this->omega_PN_B = this->omega_PB_B + this->omega_BN_B; + *this->omega_PN_P = this->dcm_BP.transpose() * this->omega_PN_B; + + Eigen::Vector3d totRotAngMomPntC_B; + totRotAngMomPntC_B.setZero(); + double totRotEnergy = 0.0; + + // Loop through attached state effectors for contributions + std::vector::iterator it; + for(it = this->stateEffectors.begin(); it != this->stateEffectors.end(); it++) { + rotAngMomPntCContr_B.setZero(); + rotEnergyContr = 0.0; + + (*it)->updateEnergyMomContributions(integTime, rotAngMomPntCContr_B, rotEnergyContr, *this->omega_PN_P); + + // Additional terms for rotational angular momentum + Eigen::Vector3d r_EcP_B = this->dcm_BP * (*it)->effProps.rEff_CB_B; + Eigen::Matrix3d rTilde_EcP_B = eigenTilde(r_EcP_B); + Eigen::Vector3d rDot_PB_B = *this->rPrime_PB_B + this->omegaTilde_BN_B * *this->r_PB_B; + Eigen::Matrix3d rTilde_PB_B = eigenTilde(*this->r_PB_B); + Eigen::Matrix3d omegaTilde_PN_B = eigenTilde(this->omega_PN_B); + Eigen::Vector3d rDot_EcP_B = this->dcm_BP * (*it)->effProps.rEffPrime_CB_B + omegaTilde_PN_B * r_EcP_B; + totRotAngMomPntC_B += this->dcm_BP * rotAngMomPntCContr_B + + (*it)->effProps.mEff * rTilde_EcP_B * rDot_PB_B + + (*it)->effProps.mEff * rTilde_PB_B * rDot_EcP_B + + (*it)->effProps.mEff * rTilde_PB_B * rDot_PB_B; + + // Additional terms for rotational energy + totRotEnergy += rotEnergyContr + + (*it)->effProps.mEff * rDot_EcP_B.dot(rDot_PB_B ) + + 0.5 * (*it)->effProps.mEff * rDot_PB_B.dot(rDot_PB_B); + } - // Compute rDot_PcB_B - this->rDot_PcB_B = this->rPrime_PcB_B + this->omegaTilde_BN_B * this->r_PcB_B; + rotAngMomPntCContr_B = totRotAngMomPntC_B; + rotEnergyContr = totRotEnergy; - // Find the rotational angular momentum contribution from hub - rotAngMomPntCContr_B = this->IPntPc_B * this->omega_PN_B + this->mass * this->rTilde_PcB_B * this->rDot_PcB_B; + // Prescribed motion rotational angular momentum contribution + this->rDot_PcB_B = this->rPrime_PcB_B + this->omegaTilde_BN_B * this->r_PcB_B; + rotAngMomPntCContr_B += this->IPntPc_B * this->omega_PN_B + this->mass * this->rTilde_PcB_B * this->rDot_PcB_B; - // Find the rotational energy contribution from the hub - rotEnergyContr = 0.5 * this->omega_PN_B.dot(this->IPntPc_B * this->omega_PN_B) + // Prescribed motion rotational energy contribution + rotEnergyContr += 0.5 * this->omega_PN_B.dot(this->IPntPc_B * this->omega_PN_B) + 0.5 * this->mass * this->rDot_PcB_B.dot(this->rDot_PcB_B); } @@ -305,13 +513,19 @@ void PrescribedMotionStateEffector::computePrescribedMotionInertialStates() { // Compute the effector's attitude with respect to the inertial frame Eigen::Matrix3d dcm_PN = (this->dcm_BP).transpose() * this->dcm_BN; - this->sigma_PN = eigenMRPd2Vector3d(eigenC2MRP(dcm_PN)); + *this->sigma_PN = eigenMRPd2Vector3d(eigenC2MRP(dcm_PN)); - // Compute the effector's inertial position vector - this->r_PcN_N = (Eigen::Vector3d)*this->inertialPositionProperty + this->dcm_BN.transpose() * this->r_PcB_B; + // Compute the effector's inertial angular velocity + *this->omega_PN_P = (this->dcm_BP).transpose() * this->omega_PN_B; - // Compute the effector's inertial velocity vector + // Compute the effector's inertial position vectors + this->r_PcN_N = (Eigen::Vector3d)(*this->inertialPositionProperty) + this->dcm_BN.transpose() * this->r_PcB_B; + *this->r_PN_N = (Eigen::Vector3d)(*this->inertialPositionProperty) + this->dcm_BN.transpose() * *this->r_PB_B; + + // Compute the effector's inertial velocity vectors this->v_PcN_N = (Eigen::Vector3d)*this->inertialVelocityProperty + this->dcm_BN.transpose() * this->rDot_PcB_B; + *this->v_PN_N = (Eigen::Vector3d)*this->inertialVelocityProperty + this->dcm_BN.transpose() * (*this->rPrime_PB_B + + this->omegaTilde_BN_B * *this->r_PB_B); } /*! This method updates the effector state at the dynamics frequency. @@ -356,3 +570,135 @@ void PrescribedMotionStateEffector::UpdateState(uint64_t currentSimNanos) // Call the method to write the output messages this->writeOutputStateMessages(currentSimNanos); } + +/*! This method attaches a stateEffector to the prescribedMotionStateEffector */ +void PrescribedMotionStateEffector::addStateEffector(StateEffector* newStateEffector) +{ + this->assignStateParamNames(newStateEffector); + + this->stateEffectors.push_back(newStateEffector); + + // Give the stateEffector the name of the prescribed object it is attached to + newStateEffector->nameOfSpacecraftAttachedTo = this->spacecraftName; +} + +/*! Setter method for the effector mass. + @param mass [kg] Effector mass +*/ +void PrescribedMotionStateEffector::setMass(const double mass) { this->mass = mass; } + +/*! Setter method for IPntPc_P. + @param IPntPc_P [kg-m^2] Effector's inertia matrix about its center of mass point Pc expressed in P frame components +*/ +void PrescribedMotionStateEffector::setIPntPc_P(const Eigen::Matrix3d IPntPc_P) { this->IPntPc_P = IPntPc_P; } + +/*! Setter method for r_PcP_P. + @param r_PcP_P [m] Position vector of the effector's center of mass point Pc relative to the effector's body frame + origin point P expressed in P frame components +*/ +void PrescribedMotionStateEffector::setR_PcP_P(const Eigen::Vector3d r_PcP_P) { this->r_PcP_P = r_PcP_P; } + +/*! Setter method for r_PM_M. + @param r_PM_M [m] Position vector of the effector's body frame origin point P relative to the hub-fixed mount frame + origin point M expressed in M frame components +*/ +void PrescribedMotionStateEffector::setR_PM_M(const Eigen::Vector3d r_PM_M) { this->r_PM_M = r_PM_M; } + +/*! Setter method for rPrime_PM_M. + @param rPrime_PM_M [m/s] B frame time derivative of r_PM_M expressed in M frame components +*/ +void PrescribedMotionStateEffector::setRPrime_PM_M(const Eigen::Vector3d rPrime_PM_M) { + this->rPrime_PM_M = rPrime_PM_M; +} + +/*! Setter method for rPrimePrime_PM_M. + @param rPrimePrime_PM_M [m/s^2] B frame time derivative of rPrime_PM_M expressed in M frame components +*/ +void PrescribedMotionStateEffector::setRPrimePrime_PM_M(const Eigen::Vector3d rPrimePrime_PM_M) { + this->rPrimePrime_PM_M = rPrimePrime_PM_M; +} + +/*! Setter method for omega_PM_P. + @param omega_PM_P [rad/s] Angular velocity of the effector body frame P relative to the hub-fixed mount frame M + expressed in P frame components +*/ +void PrescribedMotionStateEffector::setOmega_PM_P(const Eigen::Vector3d omega_PM_P) { this->omega_PM_P = omega_PM_P; } + +/*! Setter method for omegaPrime_PM_P. + @param omegaPrime_PM_P [rad/s^2] Angular acceleration of the effector body frame P relative to the hub-fixed mount + frame M expressed in P frame components +*/ +void PrescribedMotionStateEffector::setOmegaPrime_PM_P(const Eigen::Vector3d omegaPrime_PM_P) { + this->omegaPrime_PM_P = omegaPrime_PM_P; +} + +/*! Setter method for sigma_PM. + @param sigma_PM MRP attitude of the effector's body frame P relative to the hub-fixed mount frame M +*/ +void PrescribedMotionStateEffector::setSigma_PM(const Eigen::MRPd sigma_PM) { this->sigma_PM = sigma_PM; } + +/*! Setter method for r_MB_B. + @param r_MB_B [m] Position vector describing the hub-fixed mount frame origin point M location relative to the hub + frame origin point B expressed in B frame components +*/ +void PrescribedMotionStateEffector::setR_MB_B(const Eigen::Vector3d r_MB_B) { this->r_MB_B = r_MB_B; } + +/*! Setter method for sigma_MB. + @param sigma_MB MRP attitude of the hub-fixed frame M relative to the hub body frame B +*/ +void PrescribedMotionStateEffector::setSigma_MB(const Eigen::MRPd sigma_MB) { this->sigma_MB = sigma_MB; } + +/*! Getter method for the effector mass. + @return double +*/ +double PrescribedMotionStateEffector::getMass() const { return this->mass; } + +/*! Getter method for IPntPc_P. + @return const Eigen::Matrix3d +*/ +const Eigen::Matrix3d PrescribedMotionStateEffector::getIPntPc_P() const { return this->IPntPc_P; } + +/*! Getter method for r_PcP_P. + @return const Eigen::Vector3d +*/ +const Eigen::Vector3d PrescribedMotionStateEffector::getR_PcP_P() const { return this->r_PcP_P; } + +/*! Getter method for r_PM_M. + @return const Eigen::Vector3d +*/ +const Eigen::Vector3d PrescribedMotionStateEffector::getR_PM_M() const { return this->r_PM_M; } + +/*! Getter method for rPrime_PM_M. + @return const Eigen::Vector3d +*/ +const Eigen::Vector3d PrescribedMotionStateEffector::getRPrime_PM_M() const { return this->rPrime_PM_M; } + +/*! Getter method for rPrimePrime_PM_M. + @return const Eigen::Vector3d +*/ +const Eigen::Vector3d PrescribedMotionStateEffector::getRPrimePrime_PM_M() const { return this->rPrimePrime_PM_M; } + +/*! Getter method for omega_PM_P. + @return const Eigen::Vector3d +*/ +const Eigen::Vector3d PrescribedMotionStateEffector::getOmega_PM_P() const { return this->omega_PM_P; } + +/*! Getter method for omegaPrime_PM_P. + @return const Eigen::Vector3d +*/ +const Eigen::Vector3d PrescribedMotionStateEffector::getOmegaPrime_PM_P() const { return this->omegaPrime_PM_P; } + +/*! Getter method for sigma_PM. + @return const Eigen::MRPd +*/ +const Eigen::MRPd PrescribedMotionStateEffector::getSigma_PM() const { return this->sigma_PM; } + +/*! Getter method for r_MB_B. + @return const Eigen::Vector3d +*/ +const Eigen::Vector3d PrescribedMotionStateEffector::getR_MB_B() const { return this->r_MB_B; } + +/*! Getter method for sigma_MB. + @return const Eigen::MRPd +*/ +const Eigen::MRPd PrescribedMotionStateEffector::getSigma_MB() const { return this->sigma_MB; } diff --git a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.h b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.h index ba04be02d0..0095c8acb3 100644 --- a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.h +++ b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.h @@ -35,11 +35,36 @@ class PrescribedMotionStateEffector: public StateEffector, public SysModel { public: PrescribedMotionStateEffector(); ~PrescribedMotionStateEffector(); + void setMass(const double mass); //!< Setter method for the effector mass + void setIPntPc_P(const Eigen::Matrix3d IPntPc_P); //!< Setter method for IPntPc_P + void setR_PcP_P(const Eigen::Vector3d r_PcP_P); //!< Setter method for r_PcP_P + void setR_PM_M(const Eigen::Vector3d r_PM_M); //!< Setter method for r_PM_M + void setRPrime_PM_M(const Eigen::Vector3d rPrime_PM_M); //!< Setter method for rPrime_PM_M + void setRPrimePrime_PM_M(const Eigen::Vector3d rPrimePrime_PM_M); //!< Setter method for rPrimePrime_PM_M + void setOmega_PM_P(const Eigen::Vector3d omega_PM_P); //!< Setter method for omega_PM_P + void setOmegaPrime_PM_P(const Eigen::Vector3d omegaPrime_PM_P); //!< Setter method for omegaPrime_PM_P + void setSigma_PM(const Eigen::MRPd sigma_PM); //!< Setter method for sigma_PM + void setR_MB_B(const Eigen::Vector3d r_MB_B); //!< Setter method for r_MB_B + void setSigma_MB(const Eigen::MRPd sigma_MB); //!< Setter method for sigma_MB + + double getMass() const; //!< Getter method for the effector mass + const Eigen::Matrix3d getIPntPc_P() const; //!< Getter method for IPntPc_P + const Eigen::Vector3d getR_PcP_P() const; //!< Getter method for r_PcP_P + const Eigen::Vector3d getR_PM_M() const; //!< Getter method for r_PM_M + const Eigen::Vector3d getRPrime_PM_M() const; //!< Getter method for rPrime_PM_M + const Eigen::Vector3d getRPrimePrime_PM_M() const; //!< Getter method for rPrimePrime_PM_M + const Eigen::Vector3d getOmega_PM_P() const; //!< Getter method for omega_PM_P + const Eigen::Vector3d getOmegaPrime_PM_P() const; //!< Getter method for omegaPrime_PM_P + const Eigen::MRPd getSigma_PM() const; //!< Getter method for sigma_PM + const Eigen::Vector3d getR_MB_B() const; //!< Getter method for r_MB_B + const Eigen::MRPd getSigma_MB() const; //!< Getter method for sigma_MB + void Reset(uint64_t currentClock) override; //!< Method for reset void writeOutputStateMessages(uint64_t currentClock) override; //!< Method for writing the output messages void UpdateState(uint64_t currentSimNanos) override; //!< Method for updating the effector states void registerStates(DynParamManager& statesIn) override; //!< Method for registering the effector's states void linkInStates(DynParamManager& states) override; //!< Method for giving the effector access to hub states + void registerProperties(DynParamManager& states) override; //!< Method for registering the prescribed motion properties void updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, @@ -55,14 +80,20 @@ class PrescribedMotionStateEffector: public StateEffector, public SysModel { double & rotEnergyContr, Eigen::Vector3d omega_BN_B) override; //!< Method for computing the energy and momentum of the effector void computePrescribedMotionInertialStates(); //!< Method for computing the effector's states relative to the inertial frame + void addStateEffector(StateEffector *newStateEffector); //!< Method to attach a state effector to prescribed motion + + 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: double currentSimTimeSec; //!< [s] Current simulation time, updated at the dynamics frequency double mass; //!< [kg] Effector mass Eigen::Matrix3d IPntPc_P; //!< [kg-m^2] Inertia of the effector about its center of mass in P frame components Eigen::Vector3d r_MB_B; //!< [m] Position of point M relative to point B in B frame components Eigen::Vector3d r_PcP_P; //!< [m] Position of the effector center of mass relative to point P in P frame components - Eigen::Vector3d omega_MB_B; //!< [rad/s] Angular velocity of frame M with respect to frame B in B frame components - Eigen::Vector3d omegaPrime_MB_B; //!< [rad/s^2] B frame time derivative of omega_MB_B in B frame components Eigen::MRPd sigma_MB; //!< MRP attitude of frame M relative to frame B // Prescribed parameters @@ -74,18 +105,10 @@ class PrescribedMotionStateEffector: public StateEffector, public SysModel { Eigen::MRPd sigma_PM; //!< MRP attitude of frame P relative to frame M std::string nameOfsigma_PMState; //!< Identifier for the sigma_PM state data container - 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 // Given quantities from user in python Eigen::Matrix3d IPntPc_B; //!< [kg-m^2] Inertia of the effector about its center of mass in B frame components - Eigen::Vector3d r_PB_B; //!< [m] Position of point P relative to point B in B frame components Eigen::Vector3d r_PcP_B; //!< [m] Position of the effector center of mass relative to point P in B frame components // Prescribed parameters in body frame components @@ -122,8 +145,10 @@ class PrescribedMotionStateEffector: public StateEffector, public SysModel { // Effector properties relative to the inertial frame Eigen::Vector3d r_PcN_N; //!< [m] Position of frame P center of mass relative to the inertial frame in inertial frame components Eigen::Vector3d v_PcN_N; //!< [m/s] Inertial velocity of frame P center of mass relative to the inertial frame in inertial frame components - Eigen::Vector3d sigma_PN; //!< MRP attitude of frame P relative to the inertial frame - Eigen::Vector3d omega_PN_P; //!< [rad/s] Angular velocity of frame P relative to the inertial frame in P frame components + Eigen::MatrixXd* r_PN_N; //!< [m] Position of frame P relative to the inertial frame in inertial frame components + Eigen::MatrixXd* v_PN_N; //!< [m/s] Inertial velocity of frame P relative to the inertial frame in inertial frame components + Eigen::MatrixXd* sigma_PN; //!< MRP attitude of frame P relative to the inertial frame + Eigen::MatrixXd* omega_PN_P; //!< [rad/s] Angular velocity of frame P relative to the inertial frame in P frame components // Hub states Eigen::MatrixXd* inertialPositionProperty; //!< [m] r_N Inertial position relative to system spice zeroBase/refBase @@ -135,6 +160,42 @@ class PrescribedMotionStateEffector: public StateEffector, public SysModel { Eigen::Vector3d omegaEpoch_PM_P; //!< [rad/s] Angular velocity of frame P relative to frame M in P frame components StateData *sigma_PMState; //!< MRP attitude of frame P relative to frame M + // Parameters required for effector branching + std::string spacecraftName; //!< Name of prescribed object used for effector branching + std::vector stateEffectors; //!< Vector of attached state effectors + + Eigen::MatrixXd* r_PB_B; //!< [m] Position of point P relative to point B in B frame components + Eigen::MatrixXd* rPrime_PB_B; //!< [m/s] B frame time derivative of r_PB_B in B frame components + Eigen::MatrixXd* rPrimePrime_PB_B; //!< [m/s^2] B frame time derivative of rPrime_PB_B in B frame components + Eigen::MatrixXd* sigma_PB; //!< MRP attitude of frame P relative to frame B + Eigen::MatrixXd* omega_PB_P; //!< [rad/s] Angular velocity of frame P relative to frame B in P frame components + Eigen::MatrixXd* omegaPrime_PB_P; //!< [rad/s] B frame time derivative of omega_PB_P in P frame components + + std::string nameOfPrescribedPositionProperty; //!< Identifier for prescribed position r_PB_B + std::string nameOfPrescribedVelocityProperty; //!< Identifier for prescribed velocity rPrime_PB_B + std::string nameOfPrescribedAccelerationProperty; //!< Identifier for prescribed acceleration rPrimePrime_PB_B + std::string nameOfPrescribedAttitudeProperty; //!< Identifier for prescribed attitude sigma_PB + std::string nameOfPrescribedAngVelocityProperty; //!< Identifier for prescribed angular velocity omega_PB_P + std::string nameOfPrescribedAngAccelerationProperty; //!< Identifier for prescribed angular acceleration omegaPrime_PB_P + + std::string nameOfInertialPositionProperty; //!< Identifier for prescribed motion inertial position r_PN_N + std::string nameOfInertialVelocityProperty; //!< Identifier for prescribed motion inertial velocity property v_PN_N + std::string nameOfInertialAttitudeProperty; //!< Identifier for the prescribed motion inertial attitude property sigma_PN + std::string nameOfInertialAngVelocityProperty; //!< Identifier for the prescribed motion inertial angular velocity property omega_PN_P + + template + /** Assign the state engine parameter names to attached effectors*/ + void assignStateParamNames(Type effector) { + effector->setPropName_inertialPosition(this->nameOfInertialPositionProperty); + effector->setPropName_inertialVelocity(this->nameOfInertialVelocityProperty); + + effector->setPropName_prescribedPosition(this->nameOfPrescribedPositionProperty); + effector->setPropName_prescribedVelocity(this->nameOfPrescribedVelocityProperty); + effector->setPropName_prescribedAcceleration(this->nameOfPrescribedAccelerationProperty); + effector->setPropName_prescribedAttitude(this->nameOfPrescribedAttitudeProperty); + effector->setPropName_prescribedAngVelocity(this->nameOfPrescribedAngVelocityProperty); + effector->setPropName_prescribedAngAcceleration(this->nameOfPrescribedAngAccelerationProperty); + }; }; #endif /* PRESCRIBED_MOTION_STATE_EFFECTOR_H */ diff --git a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.rst b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.rst index 043792091e..8acf6c30ab 100644 --- a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.rst +++ b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.rst @@ -1,23 +1,29 @@ 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{P}`. The prescribed body is mounted onto a hub-fixed -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: +The prescribed motion state effector class is an instantiation of the state effector abstract class. This module +is used to connect a prescribed motion component to the spacecraft hub. The states of the prescribed component are +purely prescribed, meaning there are no free degrees of freedom associated with its motion and hence there are no +states integrated in this module. Instead, the states of the prescribed component are externally profiled using +a kinematic profiler module and are provided as input messages to the prescribed state effector class. + +The body frame for the prescribed body is designated by the frame :math:`\mathcal{P}`. The prescribed body is attached +to the hub through a hub-fixed mount interface described by the mount frame :math:`\mathcal{M}`. The prescribed body +hub-relative motion is profiled with respect to the mount frame :math:`\mathcal{M}`. The prescribed states are: ``r_PM_M``, ``rPrime_PM_M``, ``rPrimePrime_PM_M``, ``omega_PM_P``, ``omegaPrime_PM_P``, and ``sigma_PM``. 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. See the example script -:ref:`scenarioDeployingSolarArrays` for more information about how to set up hub-relative +input messages to profile the prescribed body's states as a function of time. If neither message is connected to the +module, the prescribed body will not move with respect to the hub. If the prescribed body should only rotate, then only +the rotational message needs to be subscribed to the module and the translational message can be ignored. Similarly, +if the prescribed component should only translate, the rotational message can be ignored. Note that either a single +profiler module can be used to connect both messages to this module, or two separate profiler modules can be used; +where one profiles the translational states and the other profiles the rotational states. +See the example script :ref:`scenarioDeployingSolarArrays` for more information about how to set up hub-relative multi-body prescribed motion using this state effector module and the associated kinematic profiler modules. + Message Connection Descriptions ------------------------------- The following table lists all the module input and output messages. The module msg variable name is set by the @@ -49,78 +55,95 @@ provides information on what this message is used for. - Output message containing the effector's inertial position and attitude states -Detailed Module Description ---------------------------- +Module Overview +--------------- + +Module Basic Capability +^^^^^^^^^^^^^^^^^^^^^^^ +The basic capability of this module is to connect a prescribed motion component to the spacecraft hub. An in-depth +discussion of the functionality of this module and the dynamics derivation required to simulate this type of component +motion is provided in the following journal paper + +.. note:: + + `"Spacecraft Backsubstitution Dynamics with General Multibody Prescribed Subcomponents" `_, + Leah Kiner, Hanspeter Schaub, and Cody Allard + Journal of Aerospace Information Systems 2025 22:8, 703-715 -Mathematical Modeling -^^^^^^^^^^^^^^^^^^^^^ -See Kiner et al.'s paper: `Spacecraft Simulation Software Implementation of General Prescribed Motion Dynamics of Two Connected Rigid Bodies `__ -for a detailed description of the derived prescribed dynamics. +Branching Capability +^^^^^^^^^^^^^^^^^^^^ +An additional feature of this module is the ability to attach other state effectors to the prescribed motion component. +Doing so creates a chain, where the prescribed component is wedged between the hub and its attached state effector. +This capability enables select component branching relative to the spacecraft hub. A detailed discussion of this +capability is provided in the following conference paper -The translational equations of motion are: +.. note:: -.. math:: - m_{\text{sc}} \left [ \ddot{\boldsymbol{r}}_{B/N} + \boldsymbol{c}^{''} + 2 \left ( \boldsymbol{\omega}_{\mathcal{B}/\mathcal{N}} \times \boldsymbol{c}^{'} \right ) + \left ( \dot{\boldsymbol{\omega}}_{\mathcal{B}/\mathcal{N}} \times \boldsymbol{c} \right ) + \boldsymbol{\omega}_{\mathcal{B}/\mathcal{N}} \times \left ( \boldsymbol{\omega}_{\mathcal{B}/\mathcal{N}} \times \boldsymbol{c} \right ) \right ] = \sum \boldsymbol{F}_{\text{ext}} + L. Kiner, and H. Schaub, `“Backsubstitution Method For Prescribed Motion Actuators With Attached Dynamic Sub-Components” `_, + AAS Astrodynamics Specialist Conference, Boston, Massachusetts, August 10–14, 2025 -The rotational equations of motion are: +Currently, this branching capability has only been configured for the spinningBodyOneDOF, spinningBodyTwoDOF, and +linearTranslationOneDOF state effectors. See the User Guide section for how to connect these effectors to the +prescribed motion effector. The scenario scripts :ref:`scenarioPrescribedMotionWithRotationBranching` +and :ref:`scenarioPrescribedMotionWithTranslationBranching` are more complex examples demonstrating how to connect +multiple state effectors to the prescribed motion effector. -.. math:: - m_{\text{sc}} [\tilde{\boldsymbol{c}}] \ddot{\boldsymbol{r}}_{B/N} + [I_{\text{sc},B}] \dot{\boldsymbol{\omega}}_{\mathcal{B}/\mathcal{N}} = \boldsymbol{L}_B - m_{\text{P}} [\tilde{\boldsymbol{r}}_{P_c/B}] \boldsymbol{r}^{''}_{P_c/B} - \left ( [I^{'}_{\text{sc},B}] + [\tilde{\boldsymbol{\omega}}_{\mathcal{B}/\mathcal{N}}][I_{\text{sc},B}] \right ) \boldsymbol{\omega}_{\mathcal{B}/\mathcal{N}} \\ - \left ( [I^{'}_{\text{P},P_c}] + [\tilde{\boldsymbol{\omega}}_{\mathcal{B}/\mathcal{N}}] [I_{\text{P},P_c}] \right ) \boldsymbol{\omega}_{\mathcal{P}/\mathcal{B}} \ - \ [I_{\text{P},P_c}] \boldsymbol{\omega}^{'}_{\mathcal{P}/\mathcal{B}} \ - \ m_{\text{P}} [\tilde{\boldsymbol{\omega}}_{\mathcal{B}/\mathcal{N}}] [\tilde{\boldsymbol{r}}_{P_c/B}] \boldsymbol{r}^{'}_{P_c/B} Module Testing +-------------- +There are two unit test scripts for this module. The first tests the basic functionality of the module, while +the second test checks the branching capability of the module. Both tests are integrated tests with the +:ref:`prescribedRotation1DOF` and :ref:`prescribedLinearTranslation` profiler modules. + +Basic Test +^^^^^^^^^^ +The basic test script for this module profiles simultaneous translation and rotation for the prescribed body relative +to the hub using the :ref:`prescribedRotation1DOF` and :ref:`prescribedLinearTranslation` profiler modules. The initial +and reference attitude and displacement of the prescribed body relative to the hub are varied in the test. The test +checks that the quantities of spacecraft orbital angular momentum, rotational angular momentum, and orbital energy +are conserved for the duration of the simulation to verify the module dynamics. + +Branching Test ^^^^^^^^^^^^^^ -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 rotation for the -prescribed body using the :ref:`prescribedRotation1DOF` profiler module. The second scenario prescribes a 1 DOF -linear translation for the prescribed body using the :ref:`prescribedLinearTranslation` profiler module. - -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_PM_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 translation is properly computed for a -series of initial and reference positions and maximum accelerations. The final prescribed position magnitude -``r_PM_M_Final`` and velocity magnitude ``rPrime_PM_M_Final`` are compared with the reference values ``r_PM_M_Ref`` -and ``rPrime_PM_M_Ref``, respectively. Additionally for each scenario, the conservation quantities of orbital angular -momentum, rotational angular momentum, and orbital energy are checked to verify the module dynamics. +The branching test script for this module has three separate test functions. All tests configure an identical spacecraft +hub an actuating prescribed motion component. The first test connects a free :ref:`spinningBodyOneDOFStateEffector` +to the prescribed component. The second test connects a free :ref:`spinningBodyTwoDOFStateEffector` to the +prescribed component. The third test connects a free :ref:`linearTranslationOneDOFStateEffector` to the prescribed +component. All tests check that the quantities of spacecraft orbital energy, orbital angular momentum, rotational +angular momentum are conserved for the duration of the simulation. + User Guide ---------- -This section is to outline the steps needed to setup a Prescribed Motion State Effector in python using Basilisk. +This section outlines how to set up the prescribed motion module in python using Basilisk. #. Import the prescribedMotionStateEffector class:: from Basilisk.simulation import prescribedMotionStateEffector -#. Create the prescribed body state effector:: +#. Create the prescribed motion state effector:: - platform = prescribedMotionStateEffector.PrescribedMotionStateEffector() + prescribed_body = prescribedMotionStateEffector.PrescribedMotionStateEffector() -#. Define the state effector module parameters:: +#. Set the prescribed motion module parameters:: - platform.mass = 100.0 - platform.IPntPc_P = [[50.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]] - platform.r_MB_B = np.array([0.0, 0.0, 0.0]) - platform.r_PcP_P = np.array([0.0, 0.0, 0.0]) - platform.r_PM_M = np.array([1.0, 0.0, 0.0]) - platform.rPrime_PM_M = np.array([0.0, 0.0, 0.0]) - platform.rPrimePrime_PM_M = np.array([0.0, 0.0, 0.0]) - platform.omega_PM_P = np.array([0.0, 0.0, 0.0]) - platform.omegaPrime_PM_P = np.array([0.0, 0.0, 0.0]) - platform.sigma_PM = np.array([0.0, 0.0, 0.0]) - platform.omega_MB_B = np.array([0.0, 0.0, 0.0]) - platform.omegaPrime_MB_B = np.array([0.0, 0.0, 0.0]) - platform.sigma_MB = np.array([0.0, 0.0, 0.0]) - platform.ModelTag = "Platform" + prescribed_body.setMass(100.0) + prescribed_body.setIPntPc_P([[50.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]]) + prescribed_body.setR_MB_B(np.array([0.0, 0.0, 0.0])) + prescribed_body.setR_PcP_P(np.array([0.0, 0.0, 0.0])) + prescribed_body.setR_PM_M(np.array([1.0, 0.0, 0.0])) + prescribed_body.setRPrime_PM_M(np.array([0.0, 0.0, 0.0])) + prescribed_body.setRPrimePrime_PM_M(np.array([0.0, 0.0, 0.0])) + prescribed_body.setOmega_PM_P(np.array([0.0, 0.0, 0.0])) + prescribed_body.setOmegaPrime_PM_P(np.array([0.0, 0.0, 0.0])) + prescribed_body.setSigma_PM(np.array([0.0, 0.0, 0.0])) + prescribed_body.setSigma_MB(np.array([0.0, 0.0, 0.0])) + prescribed_body.ModelTag = "prescribedBody" -Do this for all of the public parameters in the prescribed motion state effector module. Note that if these parameters -are not set by the user, all scalar and vector quantities are set to zero and all matrices are set to identity by -default. #. Add the prescribed state effector to your spacecraft:: - scObject.addStateEffector(platform) + scObject.addStateEffector(prescribed_body) See :ref:`spacecraft` documentation on how to set up a spacecraft object. @@ -128,7 +151,18 @@ default. #. Add the module to the task list:: - unitTestSim.AddModelToTask(unitTaskName, platform) + unitTestSim.AddModelToTask(unitTaskName, prescribed_body) See the example script :ref:`scenarioDeployingSolarArrays` for more information about how to set up hub-relative multi-body prescribed motion using this state effector module and the associated kinematic profiler modules. + +If you'd like to connect a state effector to the prescribed state effector instead of the spacecraft hub, +the states of the attached effector must be written with respect to the prescribed body frame instead of the hub +body frame. After creating the attached state effector, make sure to connect it to the prescribed motion effector +rather than the hub:: + + prescribed_body.addStateEffector(attached_state_effector) + +See the example scripts :ref:`scenarioPrescribedMotionWithRotationBranching` +and :ref:`scenarioPrescribedMotionWithTranslationBranching` for more information about how to connect +multiple state effectors to the prescribed motion effector. diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp index 8594e86fb2..3352122123 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp @@ -107,9 +107,26 @@ void SpinningBodyOneDOFStateEffector::prependSpacecraftNameToStates() /*! This method allows the SB state effector to have access to the hub states and gravity*/ void SpinningBodyOneDOFStateEffector::linkInStates(DynParamManager& statesIn) { - // - Get access to the hub's states needed for dynamic coupling - this->inertialPositionProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialPosition); - this->inertialVelocityProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialVelocity); + // Get access to the hub's states needed for dynamic coupling + this->hubOmega = statesIn.getStateObject("hubOmega"); + + // Get access to properties needed for dynamic coupling (Hub or prescribed) + this->inertialPositionProperty = statesIn.getPropertyReference(this->propName_inertialPosition); + this->inertialVelocityProperty = statesIn.getPropertyReference(this->propName_inertialVelocity); +} + +/*! This method is used to link prescribed motion properties + @return void + @param properties The parameter manager to collect from + */ +void SpinningBodyOneDOFStateEffector::linkInPrescribedMotionProperties(DynParamManager& properties) +{ + this->prescribedPositionProperty = properties.getPropertyReference(this->propName_prescribedPosition); + this->prescribedVelocityProperty = properties.getPropertyReference(this->propName_prescribedVelocity); + this->prescribedAccelerationProperty = properties.getPropertyReference(this->propName_prescribedAcceleration); + this->prescribedAttitudeProperty = properties.getPropertyReference(this->propName_prescribedAttitude); + this->prescribedAngVelocityProperty = properties.getPropertyReference(this->propName_prescribedAngVelocity); + this->prescribedAngAccelerationProperty = properties.getPropertyReference(this->propName_prescribedAngAcceleration); } /*! This method allows the SB state effector to register its states: theta and thetaDot with the dynamic parameter manager */ @@ -229,9 +246,9 @@ void SpinningBodyOneDOFStateEffector::updateContributions(double integTime, Eigen::Vector3d rDot_SB_B = this->omegaTilde_BN_B * this->r_SB_B; Eigen::Vector3d gravityTorquePntS_B = rTilde_ScS_B * this->mass * g_B; this->cTheta = (this->u - this->k * (this->theta - this->thetaRef) - this->c * (this->thetaDot - this->thetaDotRef) - + this->sHat_B.dot(gravityTorquePntS_B - omegaTilde_SN_B * IPntS_B * this->omega_SN_B - - IPntS_B * this->omegaTilde_BN_B * this->omega_SB_B - - this->mass * rTilde_ScS_B * this->omegaTilde_BN_B * rDot_SB_B)) / this->mTheta; + + this->sHat_B.dot(gravityTorquePntS_B - omegaTilde_SN_B * IPntS_B * this->omega_SN_B + - IPntS_B * this->omegaTilde_BN_B * this->omega_SB_B + - this->mass * rTilde_ScS_B * this->omegaTilde_BN_B * rDot_SB_B)) / this->mTheta; } // For documentation on contributions see Vaz Carneiro, Allard, Schaub spinning body paper @@ -239,17 +256,95 @@ void SpinningBodyOneDOFStateEffector::updateContributions(double integTime, backSubContr.matrixA = -this->mass * rTilde_ScS_B * this->sHat_B * this->aTheta.transpose(); backSubContr.matrixB = -this->mass * rTilde_ScS_B * this->sHat_B * this->bTheta.transpose(); backSubContr.vecTrans = -this->mass * this->omegaTilde_SB_B * this->rPrime_ScS_B - + this->mass * rTilde_ScS_B * this->sHat_B * this->cTheta; + + this->mass * rTilde_ScS_B * this->sHat_B * this->cTheta; // Rotation contributions backSubContr.matrixC = (this->IPntSc_B - this->mass * this->rTilde_ScB_B * rTilde_ScS_B) - * this->sHat_B * this->aTheta.transpose(); + * this->sHat_B * this->aTheta.transpose(); backSubContr.matrixD = (this->IPntSc_B - this->mass * this->rTilde_ScB_B * rTilde_ScS_B) - * this->sHat_B * this->bTheta.transpose(); + * this->sHat_B * this->bTheta.transpose(); backSubContr.vecRot = -omegaTilde_SN_B * this->IPntSc_B * this->omega_SB_B - - this->mass * this->omegaTilde_BN_B * this->rTilde_ScB_B * this->rPrime_ScB_B - - this->mass * this->rTilde_ScB_B * this->omegaTilde_SB_B * this->rPrime_ScS_B - - (this->IPntSc_B - this->mass * this->rTilde_ScB_B * rTilde_ScS_B) * this->sHat_B * this->cTheta; + - this->mass * this->omegaTilde_BN_B * this->rTilde_ScB_B * this->rPrime_ScB_B + - this->mass * this->rTilde_ScB_B * this->omegaTilde_SB_B * this->rPrime_ScS_B + - (this->IPntSc_B - this->mass * this->rTilde_ScB_B * rTilde_ScS_B) * this->sHat_B * this->cTheta; +} + +void SpinningBodyOneDOFStateEffector::addPrescribedMotionCouplingContributions(BackSubMatrices & backSubContr) { + + // Access prescribed motion properties + Eigen::Vector3d r_PB_B = (Eigen::Vector3d)*this->prescribedPositionProperty; + Eigen::Vector3d rPrime_PB_B = (Eigen::Vector3d)*this->prescribedVelocityProperty; + Eigen::Vector3d rPrimePrime_PB_B = (Eigen::Vector3d)*this->prescribedAccelerationProperty; + Eigen::MRPd sigma_PB; + sigma_PB = (Eigen::Vector3d)*this->prescribedAttitudeProperty; + Eigen::Vector3d omega_PB_P = (Eigen::Vector3d)*this->prescribedAngVelocityProperty; + Eigen::Vector3d omegaPrime_PB_P = (Eigen::Vector3d)*this->prescribedAngAccelerationProperty; + Eigen::Matrix3d dcm_PB = sigma_PB.toRotationMatrix().transpose(); + + // Collect hub states + Eigen::Vector3d omega_bN_b = this->hubOmega->getState(); + Eigen::Vector3d omega_BN_P = dcm_PB * omega_bN_b; + + // Prescribed motion translation coupling contributions + Eigen::Vector3d sHat_P = this->sHat_B; + Eigen::Vector3d r_PB_P = dcm_PB * r_PB_B; + Eigen::Matrix3d rTilde_PB_P = eigenTilde(r_PB_P); + Eigen::Vector3d r_ScS_P = this->r_ScS_B; + Eigen::Matrix3d rTilde_ScS_P = eigenTilde(r_ScS_P); + backSubContr.matrixB += this->mass * rTilde_ScS_P * sHat_P * this->aTheta.transpose() * rTilde_PB_P; + + Eigen::Matrix3d omegaTilde_PB_P = eigenTilde(omega_PB_P); + Eigen::Vector3d rPPrime_ScP_P = this->rPrime_ScB_B; + Eigen::Matrix3d omegaPrimeTilde_PB_P = eigenTilde(omegaPrime_PB_P); + Eigen::Vector3d r_ScP_P = this->r_ScB_B; + Eigen::Vector3d rPrimePrime_PB_P = dcm_PB * rPrimePrime_PB_B; + Eigen::Matrix3d omegaTilde_BN_P = eigenTilde(omega_BN_P); + Eigen::Vector3d rPrime_PB_P = dcm_PB * rPrime_PB_B; + Eigen::Vector3d term1 = - 2.0 * this->mass * omegaTilde_PB_P * rPPrime_ScP_P + - this->mass * omegaPrimeTilde_PB_P * r_ScP_P + - this->mass * omegaTilde_PB_P * omegaTilde_PB_P * r_ScP_P + - this->mass * rPrimePrime_PB_P; + double term2 = this->aTheta.transpose() * (rPrimePrime_PB_P + 2.0 * omegaTilde_BN_P * rPrime_PB_P + omegaTilde_BN_P * omegaTilde_BN_P * r_PB_P); + double term3 = this->bTheta.transpose() * (omegaPrime_PB_P + omegaTilde_BN_P * omega_PB_P); + backSubContr.vecTrans += term1 + this->mass * (term2 + term3) * rTilde_ScS_P * sHat_P; + + // Prescribed motion rotation coupling contributions + backSubContr.matrixC += - this->mass * rTilde_PB_P * rTilde_ScS_P * sHat_P * this->aTheta.transpose(); + + Eigen::Matrix3d IPntSc_P = this->IPntSc_B; + Eigen::Matrix3d rTilde_ScP_P = eigenTilde(r_ScP_P); + backSubContr.matrixD += - this->mass * rTilde_PB_P * rTilde_ScS_P * sHat_P * this->bTheta.transpose() + - (IPntSc_P - this->mass * rTilde_ScP_P * rTilde_ScS_P + - this->mass * rTilde_PB_P * rTilde_ScS_P) * sHat_P * this->aTheta.transpose() * rTilde_PB_P; + + Eigen::Vector3d r_ScB_P = r_ScP_P + r_PB_P; + Eigen::Matrix3d rTilde_ScB_P = eigenTilde(r_ScB_P); + Eigen::Vector3d omega_SP_P = this->omega_SB_B; + Eigen::Matrix3d omegaTilde_SP_P = eigenTilde(omega_SP_P); + Eigen::Matrix3d omegaTilde_SN_B = eigenTilde(this->omega_SN_B); + Eigen::Matrix3d omegaTilde_SN_P = omegaTilde_SN_B; + Eigen::Vector3d rPPrime_ScS_P = this->rPrime_ScS_B; + Eigen::Vector3d omega_PN_P = omega_PB_P + omega_BN_P; + Eigen::Matrix3d omegaTilde_PN_P = eigenTilde(omega_PN_P); + backSubContr.vecRot += - IPntSc_P * (omegaTilde_PB_P * omega_SP_P + omegaPrime_PB_P) + - omegaTilde_SN_P * IPntSc_P * omega_PB_P + - this->mass * rTilde_PB_P * omegaTilde_SP_P * rPPrime_ScS_P + - this->mass * rTilde_ScB_P * (2.0 * omegaTilde_PB_P * rPPrime_ScP_P + + omegaPrimeTilde_PB_P * r_ScP_P + + omegaTilde_PB_P * omegaTilde_PB_P * r_ScP_P + + rPrimePrime_PB_P) + + this->mass * omegaTilde_PB_P * rTilde_ScP_P * rPPrime_ScP_P + - this->mass * omegaTilde_PN_P * rTilde_PB_P * rPPrime_ScP_P + + this->mass * omegaTilde_PB_P * rTilde_PB_P * rPPrime_ScP_P + - this->mass * omegaTilde_PN_P * rTilde_ScB_P * (omegaTilde_PB_P * r_ScP_P + rPrime_PB_P) + + this->mass * omegaTilde_PB_P * rTilde_ScB_P * (omegaTilde_PB_P * r_ScP_P + rPrime_PB_P) + - this->mass * this->cTheta * rTilde_PB_P * rTilde_ScS_P * sHat_P + - ((IPntSc_P - this->mass * rTilde_ScP_P * rTilde_ScS_P + - this->mass * rTilde_PB_P * rTilde_ScS_P) * sHat_P * this->aTheta.transpose()) + * (rPrimePrime_PB_P + 2.0 * omegaTilde_BN_P * rPrime_PB_P + omegaTilde_BN_P * omegaTilde_BN_P * r_PB_P) + - ((IPntSc_P - this->mass * rTilde_ScP_P * rTilde_ScS_P + - this->mass * rTilde_PB_P * rTilde_ScS_P) * sHat_P * this->bTheta.transpose()) + * (omegaPrime_PB_P + omegaTilde_BN_P * omega_PB_P); } /*! This method is used to find the derivatives for the SB stateEffector: thetaDDot and the kinematic derivative */ @@ -293,13 +388,13 @@ void SpinningBodyOneDOFStateEffector::updateEnergyMomContributions(double integT // Compute rDot_ScB_B this->rDot_ScB_B = this->rPrime_ScB_B + this->omegaTilde_BN_B * this->r_ScB_B; - // Find rotational angular momentum contribution from hub + // Find rotational angular momentum contribution rotAngMomPntCContr_B = this->IPntSc_B * this->omega_SN_B + this->mass * this->rTilde_ScB_B * this->rDot_ScB_B; - // Find rotational energy contribution from the hub + // Find rotational energy contribution rotEnergyContr = 1.0 / 2.0 * this->omega_SN_B.dot(this->IPntSc_B * this->omega_SN_B) - + 1.0 / 2.0 * this->mass * this->rDot_ScB_B.dot(this->rDot_ScB_B) - + 1.0 / 2.0 * this->k * (this->theta - this->thetaRef) * (this->theta - this->thetaRef); + + 1.0 / 2.0 * this->mass * this->rDot_ScB_B.dot(this->rDot_ScB_B) + + 1.0 / 2.0 * this->k * (this->theta - this->thetaRef) * (this->theta - this->thetaRef); } /*! This method computes the spinning body states relative to the inertial frame */ diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h index 7525b54fa4..16cc9cb240 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h @@ -63,6 +63,7 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel { void UpdateState(uint64_t CurrentSimNanos) override; //!< -- Method for updating information void registerStates(DynParamManager& statesIn) override; //!< -- Method for registering the SB states void linkInStates(DynParamManager& states) override; //!< -- Method for getting access to other states + void linkInPrescribedMotionProperties(DynParamManager& states) override; //!< -- Method for getting access to prescribed motion properties void updateContributions(double integTime, BackSubMatrices& backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) override; //!< -- Method for back-substitution contributions @@ -73,6 +74,7 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel { double& rotEnergyContr, Eigen::Vector3d omega_BN_B) override; //!< -- Method for computing energy and momentum for SBs void prependSpacecraftNameToStates() override; //!< Method used for multiple spacecraft void computeSpinningBodyInertialStates(); //!< Method for computing the SB's states + void addPrescribedMotionCouplingContributions(BackSubMatrices& backSubContr) override; //!< Method for adding coupling contributions for state effector branching on prescribed motion private: static uint64_t effectorID; //!< [] ID number of this panel @@ -117,9 +119,19 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel { double theta = 0.0; //!< [rad] spinning body angle double thetaDot = 0.0; //!< [rad/s] spinning body angle rate Eigen::MatrixXd* inertialPositionProperty = nullptr; //!< [m] r_N inertial position relative to system spice zeroBase/refBase - Eigen::MatrixXd* inertialVelocityProperty = nullptr; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase + Eigen::MatrixXd* inertialVelocityProperty = nullptr; //!< [m/s] v_N inertial velocity relative to system spice zeroBase/refBase StateData *thetaState = nullptr; //!< -- state manager of theta for spinning body StateData *thetaDotState = nullptr; //!< -- state manager of thetaDot for spinning body + + // Properties required for prescribed motion branching/attachment + StateData* hubOmega; //!< [rad/s] hub inertial angular velocity vector + + Eigen::MatrixXd* prescribedPositionProperty = nullptr; //!< [m] r_PB_B prescribed position relative to hub + Eigen::MatrixXd* prescribedVelocityProperty = nullptr; //!< [m/s] rPrime_PB_B prescribed velocity relative to hub + Eigen::MatrixXd* prescribedAccelerationProperty = nullptr; //!< [m/s^2] rPrimePrime_PB_B prescribed acceleration relative to hub + Eigen::MatrixXd* prescribedAttitudeProperty = nullptr; //!< sigma_PB prescribed MRP attitude relative to hub + Eigen::MatrixXd* prescribedAngVelocityProperty = nullptr; //!< [rad/s] omega_PB_P prescribed angular velocity relative to hub + Eigen::MatrixXd* prescribedAngAccelerationProperty = nullptr; //!< [rad/s^2] omegaPrime_PB_P prescribed angular acceleration relative to hub }; diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp index 3fa2419be3..7865d1f040 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp @@ -143,8 +143,25 @@ void SpinningBodyTwoDOFStateEffector::prependSpacecraftNameToStates() /*! This method allows the SB state effector to have access to the hub states and gravity*/ void SpinningBodyTwoDOFStateEffector::linkInStates(DynParamManager& statesIn) { - this->inertialPositionProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialPosition); - this->inertialVelocityProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialVelocity); + // Get access to the hub's states needed for dynamic coupling + this->hubOmega = statesIn.getStateObject("hubOmega"); + + this->inertialPositionProperty = statesIn.getPropertyReference(this->propName_inertialPosition); + this->inertialVelocityProperty = statesIn.getPropertyReference(this->propName_inertialVelocity); +} + +/*! This method is used to link prescribed motion properties + @return void + @param properties The parameter manager to collect from + */ +void SpinningBodyTwoDOFStateEffector::linkInPrescribedMotionProperties(DynParamManager& properties) +{ + this->prescribedPositionProperty = properties.getPropertyReference(this->propName_prescribedPosition); + this->prescribedVelocityProperty = properties.getPropertyReference(this->propName_prescribedVelocity); + this->prescribedAccelerationProperty = properties.getPropertyReference(this->propName_prescribedAcceleration); + this->prescribedAttitudeProperty = properties.getPropertyReference(this->propName_prescribedAttitude); + this->prescribedAngVelocityProperty = properties.getPropertyReference(this->propName_prescribedAngVelocity); + this->prescribedAngAccelerationProperty = properties.getPropertyReference(this->propName_prescribedAngAcceleration); } /*! This method allows the SB state effector to register its states: theta and thetaDot with the dynamic parameter manager */ @@ -389,6 +406,137 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back - (this->IS2PntSc2_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S2_B) * this->s2Hat_B * this->CTheta.row(1); } +void SpinningBodyTwoDOFStateEffector::addPrescribedMotionCouplingContributions(BackSubMatrices & backSubContr) { + + // Access prescribed motion properties + Eigen::Vector3d r_PB_B = (Eigen::Vector3d)*this->prescribedPositionProperty; + Eigen::Vector3d rPrime_PB_B = (Eigen::Vector3d)*this->prescribedVelocityProperty; + Eigen::Vector3d rPrimePrime_PB_B = (Eigen::Vector3d)*this->prescribedAccelerationProperty; + Eigen::MRPd sigma_PB; + sigma_PB = (Eigen::Vector3d)*this->prescribedAttitudeProperty; + Eigen::Vector3d omega_PB_P = (Eigen::Vector3d)*this->prescribedAngVelocityProperty; + Eigen::Vector3d omegaPrime_PB_P = (Eigen::Vector3d)*this->prescribedAngAccelerationProperty; + Eigen::Matrix3d dcm_PB = sigma_PB.toRotationMatrix().transpose(); + + // Collect hub states + Eigen::Vector3d omega_bN_b = this->hubOmega->getState(); + Eigen::Vector3d omega_BN_P = dcm_PB * omega_bN_b; + + // Prescribed motion translation coupling contributions + Eigen::Vector3d s1Hat_P = this->s1Hat_B; + Eigen::Vector3d s2Hat_P = this->s2Hat_B; + Eigen::Vector3d r_PB_P = dcm_PB * r_PB_B; + Eigen::Matrix3d rTilde_PB_P = eigenTilde(r_PB_P); + Eigen::Vector3d r_Sc1S1_P = this->r_Sc1S1_B; + Eigen::Vector3d r_Sc2S1_P = this->r_Sc2S1_B; + Eigen::Vector3d r_Sc2S2_P = this->r_Sc2S2_B; + Eigen::Matrix3d rTilde_Sc1S1_P = eigenTilde(r_Sc1S1_P); + Eigen::Matrix3d rTilde_Sc2S1_P = eigenTilde(r_Sc2S1_P); + Eigen::Matrix3d rTilde_Sc2S2_P = eigenTilde(r_Sc2S2_P); + + backSubContr.matrixB += (this->mass1 * rTilde_Sc1S1_P + + this->mass2 * rTilde_Sc2S1_P) * s1Hat_P * this->ATheta.row(0) * rTilde_PB_P + + this->mass2 * rTilde_Sc2S2_P * s2Hat_P * this->ATheta.row(1) * rTilde_PB_P; + + Eigen::Matrix3d omegaTilde_PB_P = eigenTilde(omega_PB_P); + Eigen::Vector3d rPPrime_Sc1P_P = this->rPrime_Sc1B_B; + Eigen::Vector3d rPPrime_Sc2P_P = this->rPrime_Sc2B_B; + Eigen::Matrix3d omegaPrimeTilde_PB_P = eigenTilde(omegaPrime_PB_P); + Eigen::Vector3d r_Sc1P_P = this->r_Sc1B_B; + Eigen::Vector3d r_Sc2P_P = this->r_Sc2B_B; + Eigen::Vector3d rPrimePrime_PB_P = dcm_PB * rPrimePrime_PB_B; + Eigen::Matrix3d omegaTilde_BN_P = eigenTilde(omega_BN_P); + Eigen::Vector3d rPrime_PB_P = dcm_PB * rPrime_PB_B; + Eigen::Vector3d transTerm1 = - 2.0 * omegaTilde_PB_P * (this->mass1 * rPPrime_Sc1P_P + this->mass2 * rPPrime_Sc2P_P) + - omegaPrimeTilde_PB_P * (this->mass1 * r_Sc1P_P + this->mass2 * r_Sc2P_P) + - omegaTilde_PB_P * omegaTilde_PB_P * (this->mass1 * r_Sc1P_P + this->mass2 * r_Sc2P_P) + - (this->mass1 + this->mass2) * rPrimePrime_PB_P; + Eigen::Vector3d intTerm1 = rPrimePrime_PB_P + 2.0 * omegaTilde_BN_P * rPrime_PB_P + omegaTilde_BN_P * omegaTilde_BN_P * r_PB_P; + Eigen::Vector3d intTerm2 = omegaPrime_PB_P + omegaTilde_BN_P * omega_PB_P; + + Eigen::Vector3d transTerm2 = (this->mass1 * rTilde_Sc1S1_P + + this->mass2 * rTilde_Sc2S1_P) * (this->ATheta.row(0).dot(intTerm1) + + this->BTheta.row(0).dot(intTerm2)) * s1Hat_P; + Eigen::Vector3d transTerm3 = this->mass2 * rTilde_Sc2S2_P * (this->ATheta.row(1).dot(intTerm1) + this->BTheta.row(1).dot(intTerm2)) * s2Hat_P; + backSubContr.vecTrans += transTerm1 + transTerm2 + transTerm3; + + // Prescribed motion rotation coupling contributions + backSubContr.matrixC += - (this->mass1 * rTilde_PB_P * rTilde_Sc1S1_P + + this->mass2 * rTilde_PB_P * rTilde_Sc2S1_P ) * s1Hat_P * this->ATheta.row(0) + - this->mass2 * rTilde_PB_P * rTilde_Sc2S2_P * s2Hat_P * this->ATheta.row(1); + + Eigen::Matrix3d intTerm3 = this->mass1 * rTilde_PB_P * rTilde_Sc1S1_P + this->mass2 * rTilde_PB_P * rTilde_Sc2S1_P; + Eigen::Matrix3d IS1PntSc1_P = this->IS1PntSc1_B; + Eigen::Matrix3d IS2PntSc2_P = this->IS2PntSc2_B; + Eigen::Matrix3d rTilde_Sc1P_P = eigenTilde(r_Sc1P_P); + Eigen::Matrix3d rTilde_Sc2P_P = eigenTilde(r_Sc2P_P); + Eigen::Matrix3d intTerm4 = IS1PntSc1_P + IS2PntSc2_P + - this->mass1 * rTilde_Sc1P_P * rTilde_Sc1S1_P + - this->mass2 * rTilde_Sc2P_P * rTilde_Sc2S1_P - intTerm3; + Eigen::Matrix3d intTerm5 = this->mass2 * rTilde_PB_P * rTilde_Sc2S2_P; + Eigen::Matrix3d intTerm6 = IS2PntSc2_P - this->mass2 * rTilde_Sc2P_P * rTilde_Sc2S2_P - intTerm5; + backSubContr.matrixD += - intTerm3 * s1Hat_P * this->BTheta.row(0) + - intTerm4 * s1Hat_P * this->ATheta.row(0) * rTilde_PB_P + - intTerm5 * s2Hat_P * this->BTheta.row(1) + - intTerm6 * s2Hat_P * this->ATheta.row(1) * rTilde_PB_P; + + Eigen::Matrix3d IPPrimeS1PntSc1_P = this->IPrimeS1PntSc1_B; + Eigen::Matrix3d IPPrimeS2PntSc2_P = this->IPrimeS2PntSc2_B; + Eigen::Vector3d omega_S1P_P = this->omega_S1B_B; + Eigen::Vector3d omega_S1B_P = omega_S1P_P + omega_PB_P; + Eigen::Vector3d omega_S2P_P = this->omega_S2B_B; + Eigen::Vector3d omega_S2B_P = omega_S2P_P + omega_PB_P; + Eigen::Vector3d vecRotTerm1 = - IPPrimeS1PntSc1_P * omega_PB_P - IPPrimeS2PntSc2_P * omega_PB_P; + + Eigen::Vector3d omega_PN_P = omega_PB_P + omega_BN_P; + Eigen::Matrix3d omegaTilde_PN_P = eigenTilde(omega_PN_P); + Eigen::Vector3d vecRotTerm2 = - IS1PntSc1_P * omegaPrime_PB_P - IS2PntSc2_P * omegaPrime_PB_P + - omegaTilde_PN_P * (IS1PntSc1_P * omega_PB_P + IS2PntSc2_P * omega_PB_P); + + Eigen::Matrix3d omegaTilde_S1P_P = eigenTilde(omega_S1P_P); + Eigen::Vector3d rPPrime_Sc1S1_P = this->rPrime_Sc1S1_B; + Eigen::Vector3d intTerm7 = - this->mass1 * rTilde_PB_P * omegaTilde_S1P_P * rPPrime_Sc1S1_P; + Eigen::Vector3d rPPrime_Sc2S2_P = this->rPrime_Sc2S2_B; + Eigen::Vector3d omega_S2S1_P = this->omega_S2S1_B; + Eigen::Matrix3d omegaTilde_S2S1_P = eigenTilde(omega_S2S1_P); + Eigen::Vector3d rPPrime_Sc2S1_P = this->rPrime_Sc2S1_B; + Eigen::Vector3d intTerm8 = - this->mass2 * rTilde_PB_P * (omegaTilde_S1P_P * omegaTilde_S2S1_P * r_Sc2S2_P + + omegaTilde_S2S1_P * rPPrime_Sc2S2_P + omegaTilde_S1P_P * rPPrime_Sc2S1_P); + Eigen::Vector3d vecRotTerm3 = intTerm7 + intTerm8; + + Eigen::Vector3d r_Sc1B_P = r_Sc1P_P + r_PB_P; + Eigen::Matrix3d rTilde_Sc1B_P = eigenTilde(r_Sc1B_P); + Eigen::Vector3d intTerm9 = - this->mass1 * rTilde_Sc1B_P * (2.0 * omegaTilde_PB_P * rPPrime_Sc1P_P + + omegaPrimeTilde_PB_P * r_Sc1P_P + omegaTilde_PB_P * omegaTilde_PB_P * r_Sc1P_P + + rPrimePrime_PB_P); + Eigen::Vector3d r_Sc2B_P = r_Sc2P_P + r_PB_P; + Eigen::Matrix3d rTilde_Sc2B_P = eigenTilde(r_Sc2B_P); + Eigen::Vector3d intTerm10 = - this->mass2 * rTilde_Sc2B_P * (2.0 * omegaTilde_PB_P * rPPrime_Sc2P_P + + omegaPrimeTilde_PB_P * r_Sc2P_P + omegaTilde_PB_P * omegaTilde_PB_P * r_Sc2P_P + + rPrimePrime_PB_P); + Eigen::Vector3d vecRotTerm4 = intTerm9 + intTerm10; + + Eigen::Vector3d intTerm11 = - this->mass1 * omegaTilde_PN_P * rTilde_Sc1P_P * (omegaTilde_PB_P * r_Sc1P_P + rPrime_PB_B) + - this->mass2 * omegaTilde_PN_P * rTilde_Sc2P_P * (omegaTilde_PB_P * r_Sc2P_P + rPrime_PB_B); + Eigen::Vector3d intTerm12 = - this->mass1 * omegaTilde_PN_P * rTilde_PB_P * (rPPrime_Sc1P_P + omegaTilde_PB_P * r_Sc1P_P + rPrime_PB_P) + - this->mass2 * omegaTilde_PN_P * rTilde_PB_P * (rPPrime_Sc2P_P + omegaTilde_PB_P * r_Sc2P_P + rPrime_PB_P); + Eigen::Vector3d intTerm13 = this->mass1 * omegaTilde_PB_P * rTilde_Sc1B_P * (rPPrime_Sc1P_P + omegaTilde_PB_P * r_Sc1P_P + rPrime_PB_P) + + this->mass2 * omegaTilde_PB_P * rTilde_Sc2B_P * (rPPrime_Sc2P_P + omegaTilde_PB_P * r_Sc2P_P + rPrime_PB_P); + Eigen::Vector3d vecRotTerm5 = intTerm11 + intTerm12 + intTerm13; + + Eigen::Vector3d vecRotTerm6 = - intTerm4 * (this->ATheta.row(0).dot(intTerm1) + this->BTheta.row(0).dot(intTerm2)) * s1Hat_P + - intTerm6 * (this->ATheta.row(1).dot(intTerm1) + this->BTheta.row(1).dot(intTerm2)) * s2Hat_P; + Eigen::Vector3d vecRotTerm7 = intTerm3 * s1Hat_P * this->CTheta.row(0) + intTerm5 * s2Hat_P * this->CTheta.row(1); + + backSubContr.vecRot += vecRotTerm1 + + vecRotTerm2 + + vecRotTerm3 + + vecRotTerm4 + + vecRotTerm5 + + vecRotTerm6 + + vecRotTerm7; +} + /*! This method is used to find the derivatives for the SB stateEffector: thetaDDot and the kinematic derivative */ void SpinningBodyTwoDOFStateEffector::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) { diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h index e33e3da85d..701f3d90d7 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h @@ -77,6 +77,7 @@ class SpinningBodyTwoDOFStateEffector: public StateEffector, public SysModel { void UpdateState(uint64_t CurrentSimNanos); //!< -- Method for updating information void registerStates(DynParamManager& statesIn); //!< -- Method for registering the SB states void linkInStates(DynParamManager& states); //!< -- Method for getting access to other states + void linkInPrescribedMotionProperties(DynParamManager& states) override; //!< -- Method for getting access to prescribed motion properties void updateContributions(double integTime, BackSubMatrices& backSubContr, Eigen::Vector3d sigma_BN, @@ -93,6 +94,7 @@ class SpinningBodyTwoDOFStateEffector: public StateEffector, public SysModel { Eigen::Vector3d omega_BN_B); //!< -- Method for computing energy and momentum for SBs void prependSpacecraftNameToStates(); //!< Method used for multiple spacecraft void computeSpinningBodyInertialStates(); //!< Method for computing the SB's states + void addPrescribedMotionCouplingContributions(BackSubMatrices& backSubContr) override; //!< Method for adding coupling contributions for state effector branching on prescribed motion private: static uint64_t effectorID; //!< [] ID number of this panel @@ -173,6 +175,16 @@ class SpinningBodyTwoDOFStateEffector: public StateEffector, public SysModel { StateData *theta1DotState = nullptr; //!< -- state manager of theta1Dot for spinning body StateData* theta2State = nullptr; //!< -- state manager of theta2 for spinning body StateData* theta2DotState = nullptr; //!< -- state manager of theta2Dot for spinning body + + // Properties required for prescribed motion branching/attachment + StateData* hubOmega; //!< [rad/s] hub inertial angular velocity vector + + Eigen::MatrixXd* prescribedPositionProperty = nullptr; //!< [m] r_PB_B prescribed position relative to hub + Eigen::MatrixXd* prescribedVelocityProperty = nullptr; //!< [m/s] rPrime_PB_B prescribed velocity relative to hub + Eigen::MatrixXd* prescribedAccelerationProperty = nullptr; //!< [m/s^2] rPrimePrime_PB_B prescribed acceleration relative to hub + Eigen::MatrixXd* prescribedAttitudeProperty = nullptr; //!< sigma_PB prescribed MRP attitude relative to hub + Eigen::MatrixXd* prescribedAngVelocityProperty = nullptr; //!< [rad/s] omega_PB_P prescribed angular velocity relative to hub + Eigen::MatrixXd* prescribedAngAccelerationProperty = nullptr; //!< [rad/s^2] omegaPrime_PB_P prescribed angular acceleration relative to hub }; #endif /* SPINNING_BODY_TWO_DOF_STATE_EFFECTOR_H */ diff --git a/src/tests/test_scenarioTest.py b/src/tests/test_scenarioTest.py index 378527f49c..03c497aabd 100644 --- a/src/tests/test_scenarioTest.py +++ b/src/tests/test_scenarioTest.py @@ -72,6 +72,8 @@ , 'scenarioLambertSolver' , 'scenarioQuadMaps' , 'scenarioExtendingBoom' + , 'scenarioPrescribedMotionWithRotationBranching' + , 'scenarioPrescribedMotionWithTranslationBranching' ]) @pytest.mark.scenarioTest def test_scenarioBskScenarios(show_plots, scenarioCase):