diff --git a/docs/source/Learn/bskPrinciples.rst b/docs/source/Learn/bskPrinciples.rst index d343d502c1..1d061186fa 100644 --- a/docs/source/Learn/bskPrinciples.rst +++ b/docs/source/Learn/bskPrinciples.rst @@ -20,3 +20,4 @@ by writing a python script. bskPrinciples/bskPrinciples-8 bskPrinciples/bskPrinciples-9 bskPrinciples/bskPrinciples-10 + bskPrinciples/bskPrinciples-11 diff --git a/docs/source/Learn/bskPrinciples/bskPrinciples-11.rst b/docs/source/Learn/bskPrinciples/bskPrinciples-11.rst new file mode 100644 index 0000000000..568009ce75 --- /dev/null +++ b/docs/source/Learn/bskPrinciples/bskPrinciples-11.rst @@ -0,0 +1,309 @@ +.. _bskPrinciples-11: + +Advanced: Effector Module Branching +=================================== + +Module branching in Basilisk enables the attachment of compatible dynamic effectors on state +effectors as an alternative to attaching them directly to the hub. For example, a gimballed thruster +can be modeled as a ``thrusterDynamicEffector`` attached to a ``spinningBodyTwoDOFStateEffector``, +or a robotic docking arm can be modeled as a ``constraintDynamicEffector`` attached to a 7-DOF +revolute ``spinningBodyNDOFStateEffector``. Multiple dynamic effectors can be attached to a state +effector as well, for example applying both drag and SRP to a solar panel. + +Further details on how to set up new state effectors and dynamic effector modules for branching can +be found in :ref:`effectorBranching`. Integrated testing of effector branching is elaborated on in +:ref:`test_effectorBranching_integrated`. + +Allowable Configurations +------------------------ +The currently supported configurations of state effectors compatible as "parent" effectors and +dynamic effectors compatible as "child" effectors is summarized in the table below. Green shows +currently supported configurations, yellow shows configurations expected to be supported in the +future, and red shows configurations not planned to be supported. Additionally, blue shows the +special case of prescribed effector branching explained in :ref:`prescribedMotionStateEffector`. + +.. raw:: html + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Effector Branching Compatibility
Children Effectors
Thruster Dynamic EffectorThruster State EffectorConstraint EffectorDrag EffectorExternal Force TorqueSRP EffectorFuel Tank EffectorGravity EffectorSpinning BodiesTranslating BodiesHinged Rigid BodiesPrescribed BodiesLinear Spring Mass DamperSpherical PendulumReaction WheelVSCMG
+ Parent Effectors + Spinning Bodies 1DOF
Spinning Bodies 2DOF
Spinning Bodies NDOF
Hinged Rigid Bodies
Dual Hinged Rigid Bodies
N Hinged Rigid Bodies
Linear Translating Bodies 1DOF
Linear Translating Bodies NDOF
Prescribed Motion
+
+ +Setup +----- +Attaching a dynamic effector on a state effector is performed the same way as attaching a dynamic +effector to the hub, but instead called with the state effector. Given a setup as:: + + scSim = SimulationBaseClass.SimBaseClass() + dynProcess = scSim.CreateNewProcess(simProcessName) + dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) + + scObject = spacecraft.Spacecraft() + + stateEff = spinningBodyOneDOFStateEffector.SpinningBodyOneDOFStateEffector() + dynEff = extForceTorque.ExtForceTorque() + + scObject.addStateEffector(stateEff) + + scSim.AddModelToTask(simTaskName, scObject) + scSim.AddModelToTask(simTaskName, stateEff) + scSim.AddModelToTask(simTaskName, dynEff) + +By default, effectors attached to the hub using:: + + scObject.addDynamicEffector(dynEff) + scSim.AddModelToTask(simTaskName, dynEff) + +Are instead attached to a state effector as:: + + segment = 1 + stateEff.addDynamicEffector(dynEff, segment) + +Where segment is the integer number of the segement to be attached to, with 1 being the segment +attached to the hub, increasing outward from the hub. For example, spinningBodyOneDOFStateEffector +will always attach to segment 1, attaching to the second to last segment of a 7DOF robotic arm would +be segment 6. Note that in either case the dynamic effector is still added to the sim task. + +However, some effectors use alternative methods for adding to a spacecraft. For example, the +attachment of a :ref:`thrusterDynamicEffector` set using the :ref:`simIncludeThruster`:: + + thrusterSet = thrusterDynamicEffector.ThrusterDynamicEffector() + thFactory = simIncludeThruster.thrusterFactory() + thFactory.create('MOOG_Monarc_22_6', [0, 0, 0], [0, -1.5, 0]) + + thFactory.addToSpacecraft(thrModelTag, thrusterSet, scObject) + +Would instead be attached using:: + + thFactory.addToSpacecraftSubcomponent(thrModelTag, thrusterSet, stateEff, segment) + +Additionally, the :ref:`constraintDynamicEffector` requires attachment to two parents. It works with +either a hub attached to the state effector of another vehicle, or the attachment of two different +state effectors to one another.:: + + scObject2 = spacecraft.Spacecraft() + constraintEffector = constraintDynamicEffector.ConstraintDynamicEffector() + + stateEff.addDynamicEffector(constraintEffector) + scObject2.addDynamicEffector(constraintEffector) diff --git a/docs/source/Learn/makingModules/advancedTopics.rst b/docs/source/Learn/makingModules/advancedTopics.rst index f62ff44e1f..72441644fa 100644 --- a/docs/source/Learn/makingModules/advancedTopics.rst +++ b/docs/source/Learn/makingModules/advancedTopics.rst @@ -10,3 +10,4 @@ This section covers advanced BSK module writing topics. advancedTopics/creatingDynObject advancedTopics/mujocoDynObject + advancedTopics/effectorBranching diff --git a/docs/source/Learn/makingModules/advancedTopics/effectorBranching.rst b/docs/source/Learn/makingModules/advancedTopics/effectorBranching.rst new file mode 100644 index 0000000000..6b73c14b5e --- /dev/null +++ b/docs/source/Learn/makingModules/advancedTopics/effectorBranching.rst @@ -0,0 +1,95 @@ +.. _effectorBranching: + +Enabling Branching of Basilisk Effectors +======================================== + +Back-substitution is the default method that manages the dynamics under the hood of Basilisk. The +backsubstitution formulation assumes that all dynamical modules, a.k.a. "effectors", influencing the +dynamics of the spacecraft are attached directly to its central hub in parallel. These effectors are +sub-classed as either :ref:`dynamicEffector` (provide external forces and torques acting on the +body, but doesn't have state differential equations to integrate) or :ref:`stateEffector` (has +internal state differential equations to integrate that couple with the spacecraft rigid hub). + +This parallel coupling assumption can be relaxed to allow select configurations of effectors to be +attached in series to each other as opposed to being attached directly to the hub. See +:ref:`bskPrinciples-11` for a description on using this added functionality. + +State Effector Augmentation +--------------------------- +For a state effector, three changes must be made. + +#. Connect the state effector's properties to the :ref:`dynParamManager` + + - Ensure the effector's inertial properties are of an appropriate data type to be accepted by + the parameter manager (ex. pointer to Eigen::MatrixXd). These properties should already be + computed at each timestep by a method named something like + ``computeInertialStates()`` + - Define names for these inertial properties to be registered with in the parameter manager + - Define an ``assignStateParamNames()`` method to hand these property names to the dependent + effector + - Override the ``registerProperties()`` method of the state effector base class, and inside it + register the properties and call the corresponding ``linkInProperties()`` method of dependent + effectors + +#. Add a way to attach dynamic effectors to the state effector + + - Add a variable to the effector's class to store a list of its dependent effectors + - Override the ``addDynamicEffector()`` method of the state effector base class, and inside it + call ``assignStateParamNames()`` on the new dependent effector, and add it to the list + +#. Augment the equations of motion to apply forces and torques of dependent effectors + - Inside of ``updateContributions()``, loop over the list of dependent effectors, calling their + ``computeForceTorque()`` methods, and collecting forceExternal_B and torqueExternalPntB_B + - Insert this force and torque into the effector equation of motion, ensuring to transform it + into the appropriate frame + - Insert the force and torque into the vecTrans and vecRot contributions, ensuring to transform + it into the appropriate frame + +Dynamic Effector Augmentation +----------------------------- +For a dynamic effector, three changes must also be made. + +#. Connect the dynamic effector to its parents properties + + - Override the ``linkInProperties()`` method of the base class, and inside it collect the + relevant properties from the parameter manager. Different effectors require various combinations + of position, velocity, attitude, and angular velocity of their parent, including requiring none. + - In places where hub states are used, add a conditional to use effector properties instead if + the parent is a state effector instead of the hub. + +#. If the dynamic effector has a setup factory, such as the thruster's :ref:`simIncludeThruster` or + the reaction wheel's :ref:`simIncludeRW`, define a method there for adding to a state effector + instead of the hub. + +#. Set dynamic effector base class variable `isAttachableToStateEffector` to True in the + constructor. + +Additional Resources +-------------------- +.. important:: + The force and torque ouput by dependent effectors as variables forceExternal_B and + torqueExternalPntB_B is computed relative to its parent frame, not the body frame as they are + labeled. For example if attached to a spinningBodyOneDOFStateEffector, torqueExternalPntB_B is + actually the torque about point S expressed in the S frame (torqueExternalPntS_S) as its parent + frame is the spinning body's frame. + + This also requires that the state effector output inertial properties to this frame origin + (r_SN_N) as opposed to outputting inertial properties to its center of mass (r_ScN_N). This is + important because dynamic effectors handle converting their applied forces to torques with + respect to their parent frame origin. If the state effector passes it's center of mass position, + this torque computed by the attached dynamic effector would need to be corrected, but the state + effector does not get knowledge of where dynamic effectors are attached to make such a + correction. + +A more complete description of back-substitution, :ref:`stateEffector`, and :ref:`dynamicEffector` +can be found in :ref:`spacecraft` and its linked PDF description, particularly the Equations of +Motion section. Specifics on setting up the corresponding integrated tests for any newly enabled +branching effectors can be found in :ref:`test_effectorBranching_integrated`. A more detailed +description of this augmentation of the back-substitution method can be found in the following +conference paper. + +.. note:: + + A. Morell and H. Schaub, `"“Expanded Back-Substitution Dynamics Modeling For Branched Force And + Torque Based Spacecraft Components" `_, + AAS Spaceflight Mechanics Meeting, Kauai, HI, Jan. 19-23, 2025 diff --git a/docs/source/Support/bskReleaseNotes.rst b/docs/source/Support/bskReleaseNotes.rst index 1533ecfa28..ee5fc44529 100644 --- a/docs/source/Support/bskReleaseNotes.rst +++ b/docs/source/Support/bskReleaseNotes.rst @@ -18,7 +18,7 @@ Basilisk Release Notes - landing dynamics force/torque effector that computes the interaction between a CAD spacecraft model and a CAD asteroid or lunar surface terrain. - spacecraft charging related modules - - ability to add select branching to spacecraft effectors + - support effector branching for additional state and dynamic effectors - More effector and sensor fault modeling - `pip`-based installation and pre-compiled releases - integrating the `MuJoCo `_ library as an alternate dynamics engine @@ -26,6 +26,8 @@ Basilisk Release Notes Version |release| ----------------- +- Added :ref:`bskPrinciples-11` capability. Now enabled for :ref:`extForceTorque`, :ref:`constraintDynamicEffector`, + and :ref:`thrusterDynamicEffector` attachable to :ref:`spinningBodyOneDOFStateEffector` and :ref:`spinningBodyTwoDOFStateEffector`. - Added custom reaction wheel: "NanoAvionics RW0" to ``src/utilities/simIncludeRW.py`` - Added TLE handling utilities in :ref:`tleHandling` to parse TLE files and convert to orbital elements - Removed deprecated use of astro constants from ``src/utilities/astroFunction.py``. diff --git a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_dynamics_attached_body.py b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_dynamics_attached_body.py index 44242298f4..f2ef52ec73 100644 --- a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_dynamics_attached_body.py +++ b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_dynamics_attached_body.py @@ -46,9 +46,9 @@ def thrusterEffectorAllTests(show_plots): # provide a unique test method name, starting with test_ def test_unitThrusters(show_plots, long_angle, lat_angle, location, rate): r""" - This unit test checks the functionality of attaching a dynamic thruster to a body other than the hub. Although the - attached body is fixed with respect to the hub, the point where the thruster is attached now has an additional - offset and a different orientation. + This unit test checks the functionality of attaching a dynamic thruster to a body other than the hub using the + messaging system. Although the attached body is fixed with respect to the hub, the point where the thruster is + attached now has an additional offset and a different orientation. The unit test sets up the thruster as normal, but then converts the direction and location to take into account the attached body for testing purposes. The thruster is set to fire for the first half of the simulation, and then turn diff --git a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.cpp b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.cpp index 5cb369c01c..96a211181c 100644 --- a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.cpp +++ b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.cpp @@ -38,6 +38,9 @@ ThrusterDynamicEffector::ThrusterDynamicEffector() forceExternal_N.fill(0.0); this->stateDerivContribution.resize(1); this->stateDerivContribution.setZero(); + + /* this effector can be attached onto a state effector */ + this->isAttachableToStateEffector = true; return; } @@ -180,11 +183,19 @@ void ThrusterDynamicEffector::ConfigureThrustRequests(double currentTime) void ThrusterDynamicEffector::UpdateThrusterProperties() { // Save hub variables - Eigen::Vector3d r_BN_N = (Eigen::Vector3d)*this->inertialPositionProperty; - Eigen::Vector3d omega_BN_B = this->hubOmega->getState(); Eigen::MRPd sigma_BN; - sigma_BN = (Eigen::Vector3d)this->hubSigma->getState(); + Eigen::Vector3d omega_BN_B; + if (!this->stateNameOfSigma.empty()) { + omega_BN_B = this->hubOmega->getState(); + sigma_BN = (Eigen::Vector3d)this->hubSigma->getState(); + } + else { + omega_BN_B = *this->inertialAngVelocityProperty; + sigma_BN = (Eigen::Vector3d)*this->inertialAttitudeProperty; + } + Eigen::Matrix3d dcm_BN = (sigma_BN.toRotationMatrix()).transpose(); + Eigen::Vector3d r_BN_N = (Eigen::Vector3d)*this->inertialPositionProperty; // Define the variables related to which body the thruster is attached to. The F frame represents the platform body where the thruster attaches to Eigen::MRPd sigma_FN; @@ -242,6 +253,15 @@ void ThrusterDynamicEffector::linkInStates(DynParamManager& states){ } } +/*! This method is used to link properties to the thrusters + @param properties The parameter manager to collect from + */ +void ThrusterDynamicEffector::linkInProperties(DynParamManager& properties){ + this->inertialAttitudeProperty = properties.getPropertyReference(this->propName_inertialAttitude); + this->inertialAngVelocityProperty = properties.getPropertyReference(this->propName_inertialAngVelocity); + this->inertialPositionProperty = properties.getPropertyReference(this->propName_inertialPosition); +} + /*! This method computes the Forces on Torque on the Spacecraft Body. @param integTime Integration time @@ -250,7 +270,13 @@ void ThrusterDynamicEffector::linkInStates(DynParamManager& states){ void ThrusterDynamicEffector::computeForceTorque(double integTime, double timeStep) { // Save omega_BN_B - Eigen::Vector3d omegaLocal_BN_B = this->hubOmega->getState(); + Eigen::Vector3d omegaLocal_BN_B; + if (!this->stateNameOfSigma.empty()) { + omegaLocal_BN_B = this->hubOmega->getState(); + } + else { + omegaLocal_BN_B = *this->inertialAngVelocityProperty; + } // Force and torque variables Eigen::Vector3d SingleThrusterForce; diff --git a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.h b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.h index b3349ee8ca..cd8cbf3799 100644 --- a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.h +++ b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.h @@ -45,13 +45,14 @@ class ThrusterDynamicEffector: public SysModel, public DynamicEffector { public: ThrusterDynamicEffector(); ~ThrusterDynamicEffector(); - void linkInStates(DynParamManager& states); - void computeForceTorque(double integTime, double timeStep); - void computeStateContribution(double integTime); - void Reset(uint64_t CurrentSimNanos); + void linkInStates(DynParamManager& states) override; + void linkInProperties(DynParamManager& properties) override; + void computeForceTorque(double integTime, double timeStep) override; + void computeStateContribution(double integTime) override; + void Reset(uint64_t CurrentSimNanos) override; void addThruster(std::shared_ptr newThruster); void addThruster(std::shared_ptr newThruster, Message* bodyStateMsg); - void UpdateState(uint64_t CurrentSimNanos); + void UpdateState(uint64_t CurrentSimNanos) override; void writeOutputMessages(uint64_t CurrentClock); bool ReadInputs(); void ConfigureThrustRequests(double currentTime); @@ -75,6 +76,8 @@ class ThrusterDynamicEffector: public SysModel, public DynamicEffector { StateData *hubSigma; //!< pointer to the hub attitude states StateData *hubOmega; //!< pointer to the hub angular velocity states Eigen::MatrixXd* inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase + Eigen::MatrixXd* inertialAttitudeProperty; //!< attitude relative to inertial frame + Eigen::MatrixXd* inertialAngVelocityProperty; //!< [rad/s] inertial angular velocity relative to inertial frame BSKLogger bskLogger; //!< -- BSK Logging diff --git a/src/simulation/dynamics/_GeneralModuleFiles/_UnitTestDynamics/test_effectorBranching_integrated.py b/src/simulation/dynamics/_GeneralModuleFiles/_UnitTestDynamics/test_effectorBranching_integrated.py new file mode 100644 index 0000000000..28ef95aa48 --- /dev/null +++ b/src/simulation/dynamics/_GeneralModuleFiles/_UnitTestDynamics/test_effectorBranching_integrated.py @@ -0,0 +1,693 @@ +# 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. + + +# +# Integrated Test Script +# Purpose: Test effector branching functionality +# Author: Andrew Morell +# Creation Date: September 6, 2025 +# + +import inspect +import os +import matplotlib.pyplot as plt +import numpy as np +import pytest + +filename = inspect.getframeinfo(inspect.currentframe()).filename +path = os.path.dirname(os.path.abspath(filename)) +splitPath = path.split('simulation') + +from Basilisk.utilities import SimulationBaseClass, macros, simIncludeThruster +from Basilisk.utilities import RigidBodyKinematics as rbk +from Basilisk.architecture.bskLogging import BasiliskError +from Basilisk.architecture import sim_model +from Basilisk.simulation import spacecraft, svIntegrators, gravityEffector +from Basilisk.simulation import ( # state effectors + hingedRigidBodyStateEffector, + dualHingedRigidBodyStateEffector, + nHingedRigidBodyStateEffector, + spinningBodyOneDOFStateEffector, + spinningBodyTwoDOFStateEffector, + spinningBodyNDOFStateEffector, + linearTranslationOneDOFStateEffector, + linearTranslationNDOFStateEffector, + linearSpringMassDamper, + sphericalPendulum, + prescribedMotionStateEffector, + reactionWheelStateEffector, + vscmgStateEffector, + thrusterStateEffector, + fuelTank, +) +from Basilisk.simulation import ( # dynamic effectors + extForceTorque, + ExtPulsedTorque, + thrusterDynamicEffector, + constraintDynamicEffector, + dragDynamicEffector, + radiationPressure, + facetSRPDynamicEffector, + MtbEffector, +) +from Basilisk.architecture import messaging + +# uncomment this line if this test is to be skipped in the global unit test run, adjust message as needed +# @pytest.mark.skipif(conditionstring) +# uncomment this line if this test has an expected failure, adjust message as needed +# @pytest.mark.xfail() + +# Note: effectors commented out as True are expected to be added in the future, effectors +# commented out as False are not expected to be added in the future +@pytest.mark.parametrize("stateEffector, isParent", [ + # ("hingedRigidBodies", True), + # ("dualHingedRigidBodies", True), + # ("nHingedRigidBodies", True), + ("spinningBodiesOneDOF", True), + ("spinningBodiesTwoDOF", True), + # ("spinningBodiesNDOF", True), + # ("linearTranslationBodiesOneDOF", True), + # ("linearTranslationBodiesNDOF", True), + ("linearSpringMassDamper", False), + # ("sphericalPendulum", False), + # ("prescribedMotion", False), + # ("reactionWheels", False), + # ("VSCMGs", False), + # ("thrusterStateEffector", False), + # ("fuelTank", False), +]) +@pytest.mark.parametrize("dynamicEffector, isChild", [ + ("extForceTorque", True), + ("extPulseTorque", False), + ("thrusterDynamicEffector", True), + ("constraintEffectorOneHub", True), + ("constraintEffectorNoHubs", True), + # ("dragEffector", False), + # ("radiationPressure", False), + # ("facetSRPDynamicEffector", False), + # ("MtbEffector", False), + ("multiEffector", True), +]) + +def test_effectorBranchingIntegratedTest(show_plots, stateEffector, isParent, dynamicEffector, isChild): + r""" + **Validation Test Description** + + This integrated test sets up combinations of dynamic effector attached to a state effector. + + **Description of Variables Being Tested** + + In this file we are checking first for branching compatibility, as not every state effector is + set up to host dependent effectors (isParent), and not every dynamic effector is set up to be + attached to a state effector (isChild). The state effector compatibility is checked when the + addDynamicEffector() method is called. The dynamic effector compatibility is checked at + simulation initialization when linkInProperties() is called. + + State effectors that are expected to be able to host dynamic effectors (isParent) include: + + - :ref:`spinningBodyOneDOFStateEffector` + - :ref:`spinningBodyTwoDOFStateEffector` + + Dynamic effectors that are expected to be able to attach to state effectors (isChild) include: + + - :ref:`extForceTorque` + - :ref:`thrusterDynamicEffector` + - :ref:`constraintDynamicEffector` + + Note that the constraint effector is tested in two configurations: once where one vehicle's + state effector is attached to the hub on another vehicle (``constraintEffectorOneHub``), and + once where both vehicles' state effectors are attached to each other + (``constraintEffectorNoHubs``). Additionally, a test with multiple dynamic effectors attached + to a single state effector is included (``multiEffector``). Finally, at least one non-compatible + state effector and one non-compatible dynamic effector are included to check that the error + handling catches as expected. + + Note that the center of mass of the hub is adjusted to balance the shift in total vehicle COM + due to the addition of the state effector's mass. This adjustment is calculated such that at + simulation initialization the total center of mass of the vehicle is coincident with the hub's + body frame B (r_BN_N = r_CN_N). This greatly simplifies the setup of the constraint effector + branching scenarios. This calculation is performed by calculating the parameter mr_PcB_B in each + state effector's setup method which for the state effector's series of :math:`i` bodies is: + + .. math:: + + \sum_i m_{P_i} {}^{\mathcal{B}}\mathbf{r}_{Pc_i/B} + + Where P is the state effector's designation as "parent", with each of its :math:`i` segments + having segment body fixed frame :math:`\mathcal{P}_i`, origin :math:`P_i`, and segment center of + mass :math:`Pc_i`. Then used to compute the hub center of mass offset scObject.hub.r_BcB_B as: + + .. math:: + + {}^{\mathcal{B}}\mathbf{r}_{Bc/B} = + \frac{- \sum_i m_{P_i} {}^{\mathcal{B}}\mathbf{r}_{Pc_i/B}} {\sum_i m_{Bc}} + + In the case of a permissible combination, we then check that properties are being handed + correctly from the state effector to dynamic effector. + + These variables include: + + - ``inertialPositionProperty`` + - ``inertialVelocityProperty`` + - ``inertialAttitudeProperty`` + - ``inertialAngVelocityProperty`` + + Finally, we simultaneously check that a) the applied force and torque are being handed correctly + from the dynamic effector to state effector, and b) that these forces and torques are + implemented correctly in the state effector's equations of motion. We do this by isolating each + case where a state effector has the :ref:`extForceTorque` effector attached to it. Using the + explicitly defined ``forceExternal_B`` and ``torqueExternalPntB_B`` and the logged inertial + position and attitude properties of the state effector, we manually compute the total external + force on the vehicle about the combined center of mass and the accumulated delta V of the + vehicle's combined center of mass. The torque is then integrated using a trapezoid rule and + compared against the spacecraft's internally computed angular momentum. + + .. math:: + + {}^{\mathcal{N}}\!\Delta\mathbf{H}_{C} + & = \int_{t_0}^{t} {}^{\mathcal{N}}\!\boldsymbol{\tau}_{\text{ext},C}(t)\,dt \\ + & = \int_{t_0}^{t} \text{Pure Torque + Force Relative to COM} \\ + & = \int_{t_0}^{t} [\mathcal{NP}_j] {}^{\mathcal{P}_j}\!\boldsymbol{\tau}_{\text{ext},P_j} + + \left( {}^{\mathcal{N}}\mathbf{r}_{Pc_j/N} + - [\mathcal{NP}_j] {}^{\mathcal{P}_j}\mathbf{r}_{Pc_j/P_j} + - {}^{\mathcal{N}}\mathbf{r}_{C/N} \right) + \times \left( [\mathcal{NP}_j] {}^{\mathcal{P}_j}\mathbf{F}_{P_j} \right) + + Where :math:`j` is the segment that the dynamic effector is attached to. The sim 'truth' + :math:`{}^{\mathcal{N}}\!\Delta\mathbf{H}_{C}` (scObject.totOrbAngMomPntN_N) and + :math:`{}^{\mathcal{N}}\mathbf{r}_{C/N}` are logged from the spacecraft module. Exerted + :math:`{}^{\mathcal{P}_j}\!\boldsymbol{\tau}_{\text{ext},P_j}` and + :math:`{}^{\mathcal{P}_j}\mathbf{F}_{P_j}` are from the extForceTorque effector module. + :math:`{}^{\mathcal{N}}\mathbf{r}_{Pc_j/N}`, :math:`[\mathcal{NP}_j]`, and + :math:`{}^{\mathcal{P}_j}\mathbf{r}_{Pc_j/P_j}` come from the state effector module. + + The accumulated delta V is compared against the internally computed delta V of the spacecraft + center of mass. + + .. math:: + + {}^{\mathcal{N}}\!\Delta v_{accum,C} = \int_{t_0}^{t} \frac{[\mathcal{NP}_j] + {}^{\mathcal{P}_j}\mathbf{F}_{P_j}} {m_{Bc} + \sum_i m_{P_i}} dt + + where again :math:`j` is the segment that the dynamic effector is attached to among all + :math:`i` segments. The sim 'truth' :math:`{}^{\mathcal{N}}\!\Delta v_{accum,C}` is logged from + the spacecraft module. + """ + + effectorBranchingIntegratedTest(show_plots, stateEffector, isParent, dynamicEffector, isChild) + +def effectorBranchingIntegratedTest(show_plots, stateEffector, isParent, dynamicEffector, isChild): + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) + + # Create a sim module as an empty container + unitTestSim = SimulationBaseClass.SimBaseClass() + unitTestSim.SetProgressBar(True) + + # Create test thread + timestep = 0.001 + testProcessRate = macros.sec2nano(timestep) # update process rate update time + testProc = unitTestSim.CreateNewProcess(unitProcessName) + testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) + + # Create the spacecraft object + scObject = spacecraft.Spacecraft() + scObject.ModelTag = "spacecraftBody" + + # Set the integrator to RKF45 + integratorObject = svIntegrators.svIntegratorRKF45(scObject) + scObject.setIntegrator(integratorObject) + + # Define mass properties of the rigid hub of the spacecraft + scObject.hub.mHub = 750.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 values for the 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 = [[0.1], [0.1], [0.1]] + + # 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]) + + # Create the state effector of interest + if stateEffector == "spinningBodiesOneDOF": + stateEff, stateEffProps = setup_spinningBodiesOneDOF() + segment = 1 + elif stateEffector == "spinningBodiesTwoDOF": + stateEff, stateEffProps = setup_spinningBodiesTwoDOF() + segment = 2 + elif stateEffector == "linearSpringMassDamper": + stateEff, stateEffProps = setup_linearSpringMassDamper() + segment = 1 + else: + pytest.fail("ERROR: Effector branching integrated test using unrecognized state effector input.") + + # Compute r_BcB_B such that point B and initial total COM coincide + scObject.hub.r_BcB_B = stateEffProps.mr_PcB_B / scObject.hub.mHub + + # Create the dynamic effector of interest + if dynamicEffector == "extForceTorque": + dynamicEff = setup_extForceTorque() + elif dynamicEffector == "extPulseTorque": + dynamicEff = setup_extPulseTorque() + elif dynamicEffector == "thrusterDynamicEffector": + dynamicEff, thFactory = setup_thrusterDynamicEffector() + elif dynamicEffector == "constraintEffectorOneHub": + dynamicEff, scObjectx = setup_constraintEffectorOneHub(scObject, stateEffProps) + unitTestSim.AddModelToTask("unitTask", scObjectx) + elif dynamicEffector == "constraintEffectorNoHubs": + dynamicEff, scObjectx, stateEffx = setup_constraintEffectorNoHubs(scObject, stateEffProps) + unitTestSim.AddModelToTask("unitTask", scObjectx) + unitTestSim.AddModelToTask(unitTaskName, stateEffx) + elif dynamicEffector == "multiEffector": + dynamicEff = [setup_extForceTorque(), setup_extForceTorque()] + else: + pytest.fail("ERROR: Effector branching integrated test using unrecognized dynamic effector input.") + + # Add dynamic effector to state effector + try: + if dynamicEffector == "thrusterDynamicEffector": # if thruster, then use thruster factory + thFactory.addToSpacecraftSubcomponent("dynamicThruster", dynamicEff, stateEff, segment) + elif dynamicEffector == "multiEffector": # if multiple effectors, loop over all to add + for dynEff in dynamicEff: stateEff.addDynamicEffector(dynEff, segment) + else: + stateEff.addDynamicEffector(dynamicEff, segment) + except BasiliskError: + # check if error was meant to happen + assert not isParent, "FAILED: attempted attaching to a compatible state effector, but errored" + return + else: + # check if error wasn't meant to happen + assert isParent, "FAILED: attached to an incompatible state effector without erroring" + + # Add state effector to spacecraft + scObject.addStateEffector(stateEff) + + # Add test module to runtime call list + unitTestSim.AddModelToTask(unitTaskName, stateEff) + unitTestSim.AddModelToTask(unitTaskName, scObject) + if dynamicEffector == "multiEffector": + for dynEff in dynamicEff: unitTestSim.AddModelToTask(unitTaskName, dynEff) + else: + unitTestSim.AddModelToTask(unitTaskName, dynamicEff) + + # Log the spacecraft state message + datLog = scObject.scStateOutMsg.recorder() + unitTestSim.AddModelToTask(unitTaskName, datLog) + + # Log the effector's inertial properties + if segment == 1: + inertialPropLog = getattr(stateEff, f"{stateEffProps.inertialPropLogName}").recorder() + else: + inertialPropLog = getattr(stateEff, f"{stateEffProps.inertialPropLogName}")[segment-1].recorder() + unitTestSim.AddModelToTask(unitTaskName, inertialPropLog) + + # Add energy and momentum variables to log + scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + unitTestSim.AddModelToTask(unitTaskName, scObjectLog) + + try: + unitTestSim.InitializeSimulation() + except BasiliskError: + # check if error was meant to happen + assert not isChild, "FAILED: attempted to attach a compatible dynamic effector, but errored" + return + else: + # check if error wasn't meant to happen + assert isChild, "FAILED: attached an incompatible dynamic effector without erroring" + + # Check that properties are being handed correctly from state effector to dynamic effector + assert getDynEffInertialPropName(dynamicEffector, dynamicEff, "Position") == getStateEffInertialPropName(segment, stateEff, "Position"), ( + "FAILED: inertialPositionProperty not handed correctly between state and dynamic effectors") + assert getDynEffInertialPropName(dynamicEffector, dynamicEff, "Velocity") == getStateEffInertialPropName(segment, stateEff, "Velocity"), ( + "FAILED: inertialVelocityProperty not handed correctly between state and dynamic effectors") + assert getDynEffInertialPropName(dynamicEffector, dynamicEff, "Attitude") == getStateEffInertialPropName(segment, stateEff, "Attitude"), ( + "FAILED: inertialAttitudeProperty not handed correctly between state and dynamic effectors") + assert getDynEffInertialPropName(dynamicEffector, dynamicEff, "AngVelocity") == getStateEffInertialPropName(segment, stateEff, "AngVelocity"), ( + "FAILED: inertialAngVelocityProperty not handed correctly between state and dynamic effectors") + + # Run the sim for a few timesteps to confirm execution without error + stopTime = 1 + unitTestSim.ConfigureStopTime(macros.sec2nano(stopTime)) + unitTestSim.ExecuteSimulation() + + # Continue to check state effector EOMs using pure force & torque + if dynamicEffector != "extForceTorque": + return + + # Grab conservation quantities to compare against + rotAngMom_N = scObjectLog.totRotAngMomPntC_N # total rotational angular momentum about the total vehicle COM + totAccumDV_N = datLog.TotalAccumDV_CN_N # total accumulated deltaV of the total vehicle COM + + # Grab effector's inertial position and attitude properties + r_ScN_N_log = inertialPropLog.r_BN_N + sigma_SN_log = inertialPropLog.sigma_BN + + # Compute conservation quantities using the state and dynamic effector's logged properties + n = rotAngMom_N.shape[0]-1 # length of log minus 1 as the inertial property log lags by a timestep + extTorque = np.empty((n,3)) + dV = np.empty((n,3)) + for idx in range(n): + dcm_NS = np.transpose(rbk.MRP2C(sigma_SN_log[idx,:])) + # Compute the total accumulated deltaV + if idx == 0: + dV[idx,:] = [0.0, 0.0, 0.0] + else: + dV[idx,:] = (dV[idx-1,:] + (dcm_NS @ np.array(dynamicEff.extForce_B).flatten()) + / (scObject.hub.mHub + stateEffProps.totalMass) * timestep) + # Compute the total external torque on the vehicle + extTorque[idx,:] = (dcm_NS @ np.array(dynamicEff.extTorquePntB_B).flatten() + + np.cross(r_ScN_N_log[idx+1,:] - dcm_NS + @ np.array(stateEffProps.r_PcP_P).flatten() + - datLog.r_CN_N[idx,:], dcm_NS + @ np.array(dynamicEff.extForce_B).flatten())) + + # Integrate the torque to find accumulated change in angular momentum + dx = np.ones(n-1)*timestep + y_avg = 0.5 * (extTorque[1:] + extTorque[:-1]) + integral = np.cumsum(y_avg * dx[:, None], axis=0) + dH = np.vstack((np.zeros((1, 3)), integral)) + + # Plotting + plt.close("all") + plt.figure() + for idx in range(3): + plt.plot(scObjectLog.times() * macros.NANO2SEC, rotAngMom_N[:,idx]-rotAngMom_N[0,idx], label='$dH_{truth,' + str(idx) + '}$') + plt.plot(scObjectLog.times()[:-1] * macros.NANO2SEC, dH[:,idx], '--', label='$dH_{test,' + str(idx) + '}$') + plt.plot(scObjectLog.times() * macros.NANO2SEC, np.linalg.norm(rotAngMom_N-rotAngMom_N[0,:], axis=1), linewidth=3, label='$dH_{truth, magnitude}$') + plt.plot(scObjectLog.times()[:-1] * macros.NANO2SEC, np.linalg.norm(dH, axis=1), '--', linewidth=3, label='$dH_{test, magnitude}$') + plt.legend(loc='lower right') + plt.xlabel('Time [sec]') + plt.ylabel(r'Relative Difference $\Delta$H') + plt.title('Total Rotational Angular Momentum') + + plt.figure() + for idx in range(3): + plt.plot(datLog.times() * macros.NANO2SEC, totAccumDV_N[:,idx], label='$dV_{truth,' + str(idx) + '}$') + plt.plot(datLog.times()[:-1] * macros.NANO2SEC, dV[:,idx], "--", label='$dV_{test,' + str(idx) + '}$') + plt.plot(datLog.times() * macros.NANO2SEC, np.linalg.norm(totAccumDV_N, axis=1), linewidth=3, label='$dV_{truth, magnitude}$') + plt.plot(datLog.times()[:-1] * macros.NANO2SEC, np.linalg.norm(dV, axis=1), "--", linewidth=3, label='$dV_{test, magnitude}$') + plt.legend(loc='lower right') + plt.xlabel('Time [sec]') + plt.ylabel(r'Accumulated $\Delta$V') + plt.title('Total COM DeltaV') + + plt.figure() + for idx in range(3): + plt.plot(scObjectLog.times()[:-1] * macros.NANO2SEC, rotAngMom_N[:-1,idx]-rotAngMom_N[0,idx] - dH[:,idx], label=str(idx)) + plt.legend(loc='lower right') + plt.xlabel('Time [sec]') + plt.ylabel(r'Truth - Test $\Delta$H') + plt.title('Total Rotational Angular Momentum Change') + + plt.figure() + for idx in range(3): + plt.plot(datLog.times()[:-1] * macros.NANO2SEC, totAccumDV_N[:-1,idx] - dV[:,idx], label=str(idx)) + plt.legend(loc='lower right') + plt.xlabel('Time [sec]') + plt.ylabel(r'Truth - Test $\Delta$V') + plt.title('Total COM DeltaV Accumulated') + + if show_plots: + plt.show() + plt.close("all") + + # Check angular momentum difference against sim truth + angMom_accuracy = 1e-3 + np.testing.assert_allclose(rotAngMom_N[:-1,:]-rotAngMom_N[0,:] - dH, 0, atol=angMom_accuracy, + err_msg="angular momentum difference beyond accuracy limits") + + # Check deltaV difference against sim truth + deltaV_accuracy = 1e-6 + np.testing.assert_allclose(totAccumDV_N[:-1,:] - dV, 0, atol=deltaV_accuracy, + err_msg="deltaV difference beyond accuracy limits") + + return + +def getDynEffInertialPropName(dynamicEffector, dynamicEff, propType): + if dynamicEffector == "multiEffector": + return getattr(dynamicEff[1], f"getPropName_inertial{propType}")() + elif dynamicEffector == "constraintEffectorOneHub" or dynamicEffector == "constraintEffectorNoHubs": + propList = getattr(dynamicEff, f"getPropName_inertial{propType}")() + return propList[0] + else: + return getattr(dynamicEff, f"getPropName_inertial{propType}")() + +def getStateEffInertialPropName(segment, stateEff, propType): + if segment == 1: + return getattr(stateEff, f"nameOfInertial{propType}Property") + elif segment == 2: + return getattr(stateEff, f"nameOfInertial{propType}Property2") + +def setup_extForceTorque(): + extFT = extForceTorque.ExtForceTorque() + extFT.extForce_B = [[1.0], [1.0], [1.0]] + extFT.extTorquePntB_B = [[1.0], [1.0], [1.0]] + extFT.ModelTag = "extForceTorque" + + return(extFT) + +def setup_extPulseTorque(): + extPT = ExtPulsedTorque.ExtPulsedTorque() + extPT.countOnPulse = 1 + extPT.countOff = 1 + extPT.pulsedTorqueExternalPntB_B = [[1], [1], [1]] + extPT.ModelTag = "extPulseTorque" + + return(extPT) + +def setup_thrusterDynamicEffector(): + thruster = thrusterDynamicEffector.ThrusterDynamicEffector() + thFactory = simIncludeThruster.thrusterFactory() + thFactory.create('MOOG_Monarc_22_6', [0, 0, 0], [0, -1.5, 0]) + + thrMsgData = messaging.THRArrayOnTimeCmdMsgPayload(OnTimeRequest=[0, 0, 0]) + thrMsg = messaging.THRArrayOnTimeCmdMsg() + thrMsg.write(thrMsgData) + thruster.cmdsInMsg.subscribeTo(thrMsg) + + return(thruster, thFactory) + +def setup_constraintEffector(scObject1): + scObject2 = spacecraft.Spacecraft() + scObject2.ModelTag = "spacecraftBody2" + + # Sync dynamics integration across both spacecraft + scObject1.syncDynamicsIntegration(scObject2) + + scObject2.hub.mHub = 750.0 + scObject2.hub.IHubPntBc_B = [[600.0, 0.0, 0.0], [0.0, 600.0, 0.0], [0.0, 0.0, 600.0]] + + scObject2.gravField.gravBodies = scObject1.gravField.gravBodies + + return scObject2 + +def setup_constraintEffectorOneHub(scObjecty, stateEffProps): + constraintEffector = constraintDynamicEffector.ConstraintDynamicEffector() + constraintEffector.ModelTag = "constraintEffectorOneHub" + + scObjectx = setup_constraintEffector(scObjecty) + + # Attached to the state effector of spacecraft y and the hub of spacecraft x + r_P1Bx_Bx = [[1.0], [0.0], [0.0]] # attachment point on spacecraft x's hub Bx + r_P2S_S = [[-1.0], [0.0], [0.0]] # attachment point on spacecraft y's state effector frame S + r_P2P1_BxInit = [[1.0], [0.0], [0.0]] # connect arm between attachment points, in the S frame + + # assume r_BcB_B for spacecraft y is set s.t. r_CN_N = r_BN_N and all frames start aligned + r_BxN_N_0 = np.array(scObjecty.hub.r_CN_NInit) + stateEffProps.r_PB_B + r_P2S_S - r_P2P1_BxInit - r_P1Bx_Bx + + # let C be the frame at the combined COM of the two vehicles + r_CN_N = (np.array(scObjecty.hub.r_CN_NInit) * (scObjecty.hub.mHub + stateEffProps.totalMass) + r_BxN_N_0 + * scObjectx.hub.mHub) / (scObjecty.hub.mHub + stateEffProps.totalMass + + scObjectx.hub.mHub) + r_ByC_N = scObjecty.hub.r_CN_NInit - r_CN_N + r_BxC_N = r_BxN_N_0 - r_CN_N + + # Set the initial values for spacecraft states, augmenting angular velocity + scObjectx.hub.r_CN_NInit = r_BxN_N_0 + scObjectx.hub.v_CN_NInit = np.array(scObjecty.hub.v_CN_NInit).flatten() + np.cross(np.array(scObjecty.hub.omega_BN_BInit).flatten(),r_BxC_N.flatten()) + + scObjectx.hub.omega_BN_BInit = scObjecty.hub.omega_BN_BInit + scObjecty.hub.v_CN_NInit = np.array(scObjecty.hub.v_CN_NInit).flatten() + np.cross(np.array(scObjecty.hub.omega_BN_BInit).flatten(),r_ByC_N.flatten()) + + # Create the constraint effector module + constraintEffector = constraintDynamicEffector.ConstraintDynamicEffector() + # Set up the constraint effector + constraintEffector.ModelTag = "constraintEffector" + constraintEffector.setR_P1B1_B1(r_P1Bx_Bx) + constraintEffector.setR_P2B2_B2(r_P2S_S) + constraintEffector.setR_P2P1_B1Init(r_P2P1_BxInit) + constraintEffector.setAlpha(1E1) + constraintEffector.setBeta(1e1) + + # Add constraints to both spacecraft + scObjectx.addDynamicEffector(constraintEffector) + + return (constraintEffector, scObjectx) + +def setup_constraintEffectorNoHubs(scObjecty, stateEffPropsy): + constraintEffector = constraintDynamicEffector.ConstraintDynamicEffector() + constraintEffector.ModelTag = "constraintEffectorNoHub" + + scObjectx = setup_constraintEffector(scObjecty) + + stateEffx, stateEffPropsx = setup_spinningBodiesOneDOF() + scObjectx.addStateEffector(stateEffx) + + # Attached to the state effector of spacecraft y and the state effector of spacecraft x + r_P1Sx_Sx = [[1.0], [0.0], [0.0]] # attachment point on spacecraft x's state effector frame Sx + r_P2Sy_Sy = [[-1.0], [0.0], [0.0]] # attachment point on spacecraft y's state effector frame Sy + r_P2P1_SxInit = [[1.0], [0.0], [0.0]] # connect arm between attachment points, in the Sx frame + + # assume r_BcB_B for spacecraft x & y are set s.t. r_CN_N = r_BN_N and all frames start aligned + r_BxN_N_0 = np.array(scObjecty.hub.r_CN_NInit) + stateEffPropsy.r_PB_B + r_P2Sy_Sy - r_P2P1_SxInit - r_P1Sx_Sx - stateEffPropsx.r_PB_B + + # let C be the frame at the combined COM of the two vehicles + r_CN_N = (np.array(scObjecty.hub.r_CN_NInit) * (scObjecty.hub.mHub + stateEffPropsy.totalMass) + + r_BxN_N_0 * (scObjectx.hub.mHub + stateEffPropsx.totalMass)) / (scObjecty.hub.mHub + + stateEffPropsy.totalMass + scObjectx.hub.mHub + stateEffPropsx.totalMass) + r_ByC_N = scObjecty.hub.r_CN_NInit - r_CN_N + r_BxC_N = r_BxN_N_0 - r_CN_N + + # Set the initial values for spacecraft states, augmenting angular velocity + scObjectx.hub.r_BcB_B = stateEffPropsx.mr_PcB_B / scObjectx.hub.mHub + scObjectx.hub.r_CN_NInit = r_BxN_N_0 + scObjectx.hub.v_CN_NInit = np.array(scObjecty.hub.v_CN_NInit).flatten() + np.cross(np.array(scObjecty.hub.omega_BN_BInit).flatten(),r_BxC_N.flatten()) + + scObjectx.hub.omega_BN_BInit = scObjecty.hub.omega_BN_BInit + scObjecty.hub.v_CN_NInit = np.array(scObjecty.hub.v_CN_NInit).flatten() + np.cross(np.array(scObjecty.hub.omega_BN_BInit).flatten(),r_ByC_N.flatten()) + + # Create the constraint effector module + constraintEffector = constraintDynamicEffector.ConstraintDynamicEffector() + # Set up the constraint effector + constraintEffector.ModelTag = "constraintEffector" + constraintEffector.setR_P1B1_B1(r_P1Sx_Sx) + constraintEffector.setR_P2B2_B2(r_P2Sy_Sy) + constraintEffector.setR_P2P1_B1Init(r_P2P1_SxInit) + constraintEffector.setAlpha(1E1) + constraintEffector.setBeta(1e1) + + # Add constraints to both spacecraft + scObjectx.addDynamicEffector(constraintEffector) + + return (constraintEffector, scObjectx, stateEffx) + +def setup_spinningBodiesOneDOF(): + spinningBody = spinningBodyOneDOFStateEffector.SpinningBodyOneDOFStateEffector() + + # Define properties of spinning body + spinningBody.mass = 50.0 + spinningBody.IPntSc_S = [[50.0, 0.0, 0.0], [0.0, 30.0, 0.0], [0.0, 0.0, 40.0]] + spinningBody.dcm_S0B = [[0.0, -1.0, 0.0], [0.0, .0, -1.0], [1.0, 0.0, 0.0]] + spinningBody.r_ScS_S = [[1.0], [0.0], [-1.0]] + spinningBody.r_SB_B = [[0.5], [-1.5], [-0.5]] + spinningBody.sHat_S = [[0], [-1], [0]] + spinningBody.thetaInit = 5.0 * macros.D2R + spinningBody.thetaDotInit = -1.0 * macros.D2R + spinningBody.k = 100.0 + spinningBody.c = 50 + spinningBody.ModelTag = "SpinningBody" + + # Compute COM offset contribution, to be divided by the hub mass + mr_ScB_B = -(spinningBody.r_SB_B + np.transpose(spinningBody.dcm_S0B) @ spinningBody.r_ScS_S) * spinningBody.mass + + stateEffProps = stateEfectorProperties() + stateEffProps.totalMass = spinningBody.mass + stateEffProps.mr_PcB_B = mr_ScB_B + stateEffProps.r_PB_B = spinningBody.r_SB_B + stateEffProps.r_PcP_P = spinningBody.r_ScS_S + stateEffProps.inertialPropLogName = "spinningBodyConfigLogOutMsg" + + return(spinningBody, stateEffProps) + +def setup_spinningBodiesTwoDOF(): + spinningBody = spinningBodyTwoDOFStateEffector.SpinningBodyTwoDOFStateEffector() + + # Define properties of spinning body + spinningBody.mass1 = 100.0 + spinningBody.mass2 = 50.0 + spinningBody.IS1PntSc1_S1 = [[100.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]] + spinningBody.IS2PntSc2_S2 = [[50.0, 0.0, 0.0], [0.0, 30.0, 0.0], [0.0, 0.0, 40.0]] + spinningBody.dcm_S10B = [[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, 1.0]] + spinningBody.dcm_S20S1 = [[0.0, -1.0, 0.0], [0.0, .0, -1.0], [1.0, 0.0, 0.0]] + spinningBody.r_Sc1S1_S1 = [[2.0], [-0.5], [0.0]] + spinningBody.r_Sc2S2_S2 = [[1.0], [0.0], [-1.0]] + spinningBody.r_S1B_B = [[-2.0], [0.5], [-1.0]] + spinningBody.r_S2S1_S1 = [[0.5], [-1.5], [-0.5]] + spinningBody.s1Hat_S1 = [[0], [0], [1]] + spinningBody.s2Hat_S2 = [[0], [-1], [0]] + spinningBody.theta1DotInit = 1.0 * macros.D2R + spinningBody.theta2DotInit = 1.0 * macros.D2R + spinningBody.k1 = 1000.0 + spinningBody.k2 = 500.0 + spinningBody.c1 = 500 + spinningBody.c2 = 200 + spinningBody.ModelTag = "SpinningBody" + + # Compute COM offset contribution, to be divided by the hub mass + mr_ScB_B = -( (spinningBody.r_S1B_B + np.transpose(spinningBody.dcm_S10B) @ + spinningBody.r_Sc1S1_S1) * spinningBody.mass1 + (spinningBody.r_S1B_B + + np.transpose(spinningBody.dcm_S10B) @ (spinningBody.r_S2S1_S1 + + np.transpose(spinningBody.dcm_S20S1) @ spinningBody.r_Sc2S2_S2) ) + * spinningBody.mass2) + + stateEffProps = stateEfectorProperties() + stateEffProps.totalMass = spinningBody.mass1 + spinningBody.mass2 + stateEffProps.mr_PcB_B = mr_ScB_B + stateEffProps.r_PB_B = spinningBody.r_S1B_B + np.transpose(spinningBody.dcm_S10B) @ spinningBody.r_S2S1_S1 + stateEffProps.r_PcP_P = spinningBody.r_Sc2S2_S2 + stateEffProps.inertialPropLogName = "spinningBodyConfigLogOutMsgs" + + return(spinningBody, stateEffProps) + +def setup_linearSpringMassDamper(): + linearSpring = linearSpringMassDamper.LinearSpringMassDamper() + linearSpring.massInit = 50.0 + linearSpring.k = 100.0 + linearSpring.c = 50.0 + linearSpring.r_PB_B = [[1.0], [0.0], [0.0]] + linearSpring.pHat_B = [[1.0], [0.0], [0.0]] + linearSpring.rhoInit = 0.0 + linearSpring.rhoDotInit = 0.5 + linearSpring.ModelTag = "linearSpringMassDamper" + + # Compute COM offset contribution, to be divided by the hub mass + mr_ScB_B = -(linearSpring.r_PB_B + linearSpring.rhoInit * np.array(linearSpring.pHat_B)) * linearSpring.massInit + + stateEffProps = stateEfectorProperties() + stateEffProps.totalMass = linearSpring.massInit + stateEffProps.mr_PcB_B = mr_ScB_B + stateEffProps.r_PB_B = linearSpring.r_PB_B + + return(linearSpring, stateEffProps) + +class stateEfectorProperties: + # to be used in joint COM calculation + totalMass = 0.0 # total mass of the effector (sum of all linkages) + mr_PcB_B = [[0.0], [0.0], [0.0]] # sum(m_i * r_SiB_B) for i linkages, see rst documentation + r_PB_B = [[0.0], [0.0], [0.0]] # frame origin for linkage that dynEff will be attached to + # to be used in checking equations of motion + inertialPropLogName = "" # name of inertial property output log message + r_PcP_P = [[0.0], [0.0], [0.0]] # individual COM for linkage that dynEff will be attached to + +if __name__ == "__main__": + effectorBranchingIntegratedTest(True, "spinningBodiesTwoDOF", True, "constraintEffectorOneHub", True) diff --git a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.cpp b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.cpp index 529fee66c5..79b2b159ea 100755 --- a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.cpp @@ -43,6 +43,15 @@ void DynamicEffector::computeStateContribution(double integTime) return; } +/*! This method is used to link in properties from a parent body that isn't the hub */ +void DynamicEffector::linkInProperties(DynParamManager& properties) +{ + // check that this effector can be attached to a state effector + if (!this->isAttachableToStateEffector) { + bskLogger.bskLog(BSK_ERROR, "DynamicEffector: This effector cannot be attached to a state effector."); + } +} + void DynamicEffector::setStateNameOfPosition(std::string value) { // check that value is acceptable @@ -173,6 +182,26 @@ void DynamicEffector::setPropName_inertialVelocity(std::string value) } } +void DynamicEffector::setPropName_inertialAttitude(std::string value) +{ + // check that value is acceptable + if (!value.empty()) { + this->propName_inertialAttitude = value; + } else { + bskLogger.bskLog(BSK_ERROR, "DynamicEffector: propName_inertialAttitude variable must be a non-empty string"); + } +} + +void DynamicEffector::setPropName_inertialAngVelocity(std::string value) +{ + // check that value is acceptable + if (!value.empty()) { + this->propName_inertialAngVelocity = value; + } else { + bskLogger.bskLog(BSK_ERROR, "DynamicEffector: propName_inertialAngVelocity variable must be a non-empty string"); + } +} + void DynamicEffector::setPropName_vehicleGravity(std::string value) { // check that value is acceptable diff --git a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h index ac01c0b086..a4cf78dc2a 100755 --- a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h @@ -31,6 +31,7 @@ class DynamicEffector { virtual ~DynamicEffector(); //!< Destructor virtual void computeStateContribution(double integTime); virtual void linkInStates(DynParamManager& states) = 0; //!< Method to get access to other states/stateEffectors + virtual void linkInProperties(DynParamManager& properties); //!< Method to get access to other properties/stateEffectors virtual void computeForceTorque(double integTime, double timeStep) = 0; //!< -- Method to computeForce and torque on the body public: @@ -40,19 +41,19 @@ class DynamicEffector { Eigen::Vector3d torqueExternalPntB_B = Eigen::Vector3d::Zero(); //!< [Nm] External torque applied by this effector /** setter for `stateNameOfPosition` property */ - void setStateNameOfPosition(std::string value); + virtual void setStateNameOfPosition(std::string value); /** getter for `stateNameOfPosition` property */ const std::string getStateNameOfPosition() const {return this->stateNameOfPosition; } /** setter for `stateNameOfVelocity` property */ - void setStateNameOfVelocity(std::string value); + virtual void setStateNameOfVelocity(std::string value); /** getter for `stateNameOfVelocity` property */ const std::string getStateNameOfVelocity() const { return this->stateNameOfVelocity; } /** setter for `stateNameOfSigma` property */ - void setStateNameOfSigma(std::string value); + virtual void setStateNameOfSigma(std::string value); /** getter for `stateNameOfSigma` property */ const std::string getStateNameOfSigma() const { return this->stateNameOfSigma; } /** setter for `stateNameOfOmega` property */ - void setStateNameOfOmega(std::string value); + virtual void setStateNameOfOmega(std::string value); /** getter for `stateNameOfOmega` property */ const std::string getStateNameOfOmega() const { return this->stateNameOfOmega; } /** setter for `propName_m_SC` property */ @@ -84,13 +85,21 @@ class DynamicEffector { /** getter for `propName_centerOfMassDotSC` property */ const std::string getPropName_centerOfMassDotSC() const { return this->propName_centerOfMassDotSC; } /** setter for `propName_inertialPosition` property */ - void setPropName_inertialPosition(std::string value); + virtual void setPropName_inertialPosition(std::string value); /** getter for `propName_inertialPosition` property */ const std::string getPropName_inertialPosition() const { return this->propName_inertialPosition; } /** setter for `propName_inertialVelocity` property */ - void setPropName_inertialVelocity(std::string value); + virtual void setPropName_inertialVelocity(std::string value); /** getter for `propName_inertialVelocity` property */ const std::string getPropName_inertialVelocity() const { return this->propName_inertialVelocity; } + /** setter for `propName_inertialAttitude` property */ + virtual void setPropName_inertialAttitude(std::string value); + /** getter for `propName_inertialAttitude` property */ + const std::string getPropName_inertialAttitude() const { return this->propName_inertialAttitude; } + /** setter for `propName_inertialAngVelocity` property */ + virtual void setPropName_inertialAngVelocity(std::string value); + /** getter for `propName_inertialAngVelocity` property */ + const std::string getPropName_inertialAngVelocity() const { return this->propName_inertialAngVelocity; } /** setter for `propName_vehicleGravity` property */ void setPropName_vehicleGravity(std::string value); /** getter for `propName_vehicleGravity` property */ @@ -113,8 +122,11 @@ class DynamicEffector { std::string propName_centerOfMassDotSC = ""; //!< property name of centerOfMassDotSC std::string propName_inertialPosition = ""; //!< property name of inertialPosition std::string propName_inertialVelocity = ""; //!< property name of inertialVelocity + std::string propName_inertialAttitude = ""; //!< property name of inertialAttitude + std::string propName_inertialAngVelocity = ""; //!< property name of inertialAngVelocity std::string propName_vehicleGravity = ""; //!< property name of vehicleGravity + bool isAttachableToStateEffector = false; //!< Whether or not this effector can be attached onto a state effector }; diff --git a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp index 2d1de789f7..85f8a2a9e1 100755 --- a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp @@ -95,6 +95,18 @@ void StateEffector::writeOutputStateMessages(uint64_t integTimeNanos) return; } +/*! This method allows the effector to register its properties */ +void StateEffector::registerProperties(DynParamManager& states) +{ + return; +} + +/*! This method can only be called for a state effector with override definition set up to support attached dynamic effectors */ +void StateEffector::addDynamicEffector(DynamicEffector *newDynamicEffector, int segment) +{ + bskLogger.bskLog(BSK_ERROR, "StateEffector: This effector is not compatable with attached effectors"); +} + /*! This method ensures that stateEffectors can be implemented using the multi-spacecraft archticture */ void StateEffector::prependSpacecraftNameToStates() { diff --git a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h index a5d1c6b777..8f6510aec2 100755 --- a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h @@ -24,6 +24,7 @@ #include "architecture/utilities/avsEigenMRP.h" #include "dynParamManager.h" #include "architecture/utilities/bskLogging.h" +#include "simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h" /*! back substitution matrix structure*/ @@ -130,6 +131,8 @@ class StateEffector { 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 addDynamicEffector(DynamicEffector *newDynamicEffector, int segment); //!< Method to attach a dynamic effector virtual void linkInStates(DynParamManager& states) = 0; //!< Method for stateEffectors to get other states 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(); diff --git a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp index 261556d868..7c81fcbef0 100644 --- a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp +++ b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp @@ -23,10 +23,11 @@ #include #include -/*! This is the constructor, nothing to report here */ +/*! The Constructor */ ConstraintDynamicEffector::ConstraintDynamicEffector() { - + /* this effector can be attached onto a state effector */ + this->isAttachableToStateEffector = true; } /*! This is the destructor, nothing to report here */ @@ -35,9 +36,7 @@ ConstraintDynamicEffector::~ConstraintDynamicEffector() } -/*! This method is used to reset the module. - - */ +/*! This method is used to reset the module */ void ConstraintDynamicEffector::Reset(uint64_t CurrentSimNanos) { // check if any individual gains are not specified @@ -127,8 +126,73 @@ void ConstraintDynamicEffector::setC_a(double c_a) { } } -/*! This method allows the user to set the cut-off frequency of the low pass filter which is then used to calculate the coefficients for numerical low pass filtering based on a second-order low pass filter design. +void ConstraintDynamicEffector::setStateNameOfPosition(std::string value) { + if (!value.empty()) { + this->stateNameOfPosition.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: stateNameOfPosition variable must be a non-empty string"); + } +} + +void ConstraintDynamicEffector::setStateNameOfVelocity(std::string value) { + if (!value.empty()) { + this->stateNameOfVelocity.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: stateNameOfVelocity variable must be a non-empty string"); + } +} + +void ConstraintDynamicEffector::setStateNameOfSigma(std::string value) { + if (!value.empty()) { + this->stateNameOfSigma.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: stateNameOfSigma variable must be a non-empty string"); + } +} + +void ConstraintDynamicEffector::setStateNameOfOmega(std::string value) { + if (!value.empty()) { + this->stateNameOfOmega.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: stateNameOfOmega variable must be a non-empty string"); + } +} +void ConstraintDynamicEffector::setPropName_inertialPosition(std::string value) { + if (!value.empty()) { + this->propName_inertialPosition.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: propName_inertialPosition variable must be a non-empty string"); + } +} + +void ConstraintDynamicEffector::setPropName_inertialVelocity(std::string value) { + if (!value.empty()) { + this->propName_inertialVelocity.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: propName_inertialVelocity variable must be a non-empty string"); + } +} + +void ConstraintDynamicEffector::setPropName_inertialAttitude(std::string value) { + if (!value.empty()) { + this->propName_inertialAttitude.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: propName_inertialAttitude variable must be a non-empty string"); + } +} + +void ConstraintDynamicEffector::setPropName_inertialAngVelocity(std::string value) { + if (!value.empty()) { + this->propName_inertialAngVelocity.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: propName_inertialAngVelocity variable must be a non-empty string"); + } +} + +/*! This method allows the user to set the cut-off frequency of the low pass filter which is then + used to calculate the coefficients for numerical low pass filtering based on a second-order low + pass filter design. @param wc The cut-off frequency of the low pass filter. @param h The constant digital time step. @param k The damping coefficient @@ -148,40 +212,89 @@ void ConstraintDynamicEffector::setFilter_Data(double wc, double h, double k){ } } -/*! This method allows the user to set the status of the constraint dynamic effector - -*/ +/*! This method allows the user to set the status of the constraint dynamic effector */ void ConstraintDynamicEffector::readInputMessage(){ - if(this->effectorStatusInMsg.isLinked()){ - DeviceStatusMsgPayload statusMsg; - statusMsg = this->effectorStatusInMsg(); - this->effectorStatus = statusMsg.deviceStatus; - } - else{ - this->effectorStatus = 1; - } + if(this->effectorStatusInMsg.isLinked()){ + DeviceStatusMsgPayload statusMsg; + statusMsg = this->effectorStatusInMsg(); + this->effectorStatus = statusMsg.deviceStatus; + } + else{ + this->effectorStatus = 1; + } } /*! This method allows the constraint effector to have access to the parent states - @param states The states to link */ void ConstraintDynamicEffector::linkInStates(DynParamManager& states) { if (this->scInitCounter > 1) { - bskLogger.bskLog(BSK_ERROR, "constraintDynamicEffector: tried to attach more than 2 spacecraft"); + bskLogger.bskLog(BSK_ERROR, "constraintDynamicEffector: tried to attach more than 2 parents"); + } + + int stateIdx = -1; + if (this->scInitCounter == 0) { + this->parent1.parentType = "hub"; + this->parent1.idx = 0; + this->hubCounter = 1; + stateIdx = this->parent1.idx; + } + else if (this->scInitCounter == 1) { + this->parent2.parentType = "hub"; + if (this->hubCounter == 0) { + this->parent2.idx = 0; + } else if (this->hubCounter == 1) { + this->parent2.idx = 1; + } + stateIdx = this->parent2.idx; } - this->hubSigma.push_back(states.getStateObject("hubSigma")); - this->hubOmega.push_back(states.getStateObject("hubOmega")); - this->hubPosition.push_back(states.getStateObject("hubPosition")); - this->hubVelocity.push_back(states.getStateObject("hubVelocity")); + this->hubSigma.push_back(states.getStateObject(this->stateNameOfSigma[stateIdx])); + this->hubOmega.push_back(states.getStateObject(this->stateNameOfOmega[stateIdx])); + this->hubPosition.push_back(states.getStateObject(this->stateNameOfPosition[stateIdx])); + this->hubVelocity.push_back(states.getStateObject(this->stateNameOfVelocity[stateIdx])); this->scInitCounter++; } -/*! This method computes the forces on torques on each spacecraft body. +/*! This method is used to link properties to the constraint effector. + @param properties The parameter manager to collect from + */ +void ConstraintDynamicEffector::linkInProperties(DynParamManager& properties){ + if (this->scInitCounter > 1) { + bskLogger.bskLog(BSK_ERROR, "constraintDynamicEffector: tried to attach more than 2 parents"); + } + int propIdx = -1; + if (this->scInitCounter == 0) { + this->parent1.parentType = "effector"; + this->parent1.idx = 0; + this->effectorCounter = 1; + propIdx = this->parent1.idx; + } + else if (this->scInitCounter == 1) { + this->parent2.parentType = "effector"; + if (this->effectorCounter == 0) { + this->parent2.idx = 0; + // if a spacecraft is added first, it propulated the pos and vel properties + this->propName_inertialPosition.erase(this->propName_inertialPosition.begin()); + this->propName_inertialVelocity.erase(this->propName_inertialVelocity.begin()); + } else if (this->effectorCounter == 1) { + this->parent2.idx = 1; + } + propIdx = this->parent2.idx; + } + + this->inertialAttitudeProperty.push_back(properties.getPropertyReference(this->propName_inertialAttitude[propIdx])); + this->inertialAngVelocityProperty.push_back(properties.getPropertyReference(this->propName_inertialAngVelocity[propIdx])); + this->inertialPositionProperty.push_back(properties.getPropertyReference(this->propName_inertialPosition[propIdx])); + this->inertialVelocityProperty.push_back(properties.getPropertyReference(this->propName_inertialVelocity[propIdx])); + + this->scInitCounter++; +} + +/*! This method computes the forces on torques on each spacecraft body. @param integTime Integration time @param timeStep Current integration time step used */ @@ -190,17 +303,38 @@ void ConstraintDynamicEffector::computeForceTorque(double integTime, double time if (this->scInitCounter == 2) { // only proceed once both spacecraft are added // alternate assigning the constraint force and torque if (this->scID == 0) { // compute all forces and torques once, assign to spacecraft 1 and store for spacecraft 2 - // - Collect states from both spacecraft - Eigen::Vector3d r_B1N_N = this->hubPosition[0]->getState(); - Eigen::Vector3d rDot_B1N_N = this->hubVelocity[0]->getState(); - Eigen::Vector3d omega_B1N_B1 = this->hubOmega[0]->getState(); + Eigen::Vector3d r_B1N_N; + Eigen::Vector3d rDot_B1N_N; + Eigen::Vector3d omega_B1N_B1; Eigen::MRPd sigma_B1N; - sigma_B1N = (Eigen::Vector3d)this->hubSigma[0]->getState(); - Eigen::Vector3d r_B2N_N = this->hubPosition[1]->getState(); - Eigen::Vector3d rDot_B2N_N = this->hubVelocity[1]->getState(); - Eigen::Vector3d omega_B2N_B2 = this->hubOmega[1]->getState(); + Eigen::Vector3d r_B2N_N; + Eigen::Vector3d rDot_B2N_N; + Eigen::Vector3d omega_B2N_B2; Eigen::MRPd sigma_B2N; - sigma_B2N = (Eigen::Vector3d)this->hubSigma[1]->getState(); + if (this->parent1.parentType == "hub") { + // - Collect states from parent spacecraft hub + r_B1N_N = this->hubPosition[parent1.idx]->getState(); + rDot_B1N_N = this->hubVelocity[parent1.idx]->getState(); + omega_B1N_B1 = this->hubOmega[parent1.idx]->getState(); + sigma_B1N = (Eigen::Vector3d)this->hubSigma[parent1.idx]->getState(); + } else if (this->parent2.parentType == "effector") { + // - Collect properties from parent effector + r_B1N_N = *this->inertialPositionProperty[parent1.idx]; + rDot_B1N_N = *this->inertialVelocityProperty[parent1.idx]; + omega_B1N_B1 = *this->inertialAngVelocityProperty[parent1.idx]; + sigma_B1N = (Eigen::Vector3d)*this->inertialAttitudeProperty[parent1.idx]; + } + if (this->parent2.parentType == "hub") { + r_B2N_N = this->hubPosition[parent2.idx]->getState(); + rDot_B2N_N = this->hubVelocity[parent2.idx]->getState(); + omega_B2N_B2 = this->hubOmega[parent2.idx]->getState(); + sigma_B2N = (Eigen::Vector3d)this->hubSigma[parent2.idx]->getState(); + } else if (this->parent2.parentType == "effector") { + r_B2N_N = *this->inertialPositionProperty[parent2.idx]; + rDot_B2N_N = *this->inertialVelocityProperty[parent2.idx]; + omega_B2N_B2 = *this->inertialAngVelocityProperty[parent2.idx]; + sigma_B2N = (Eigen::Vector3d)*this->inertialAttitudeProperty[parent2.idx]; + } // computing direction constraint psi in the N frame Eigen::Matrix3d dcm_B1N = (sigma_B1N.toRotationMatrix()).transpose(); @@ -256,9 +390,8 @@ void ConstraintDynamicEffector::computeForceTorque(double integTime, double time } } -/*! This method takes the computed constraint force and torque states and outputs them to the m +/*! This method takes the computed constraint force and torque states and outputs them to the messaging system. - @param CurrentClock The current simulation time (used for time stamping) */ void ConstraintDynamicEffector::writeOutputStateMessage(uint64_t CurrentClock) @@ -276,7 +409,6 @@ void ConstraintDynamicEffector::writeOutputStateMessage(uint64_t CurrentClock) } /*! Update state method - @param CurrentSimNanos The current simulation time */ void ConstraintDynamicEffector::UpdateState(uint64_t CurrentSimNanos) @@ -290,12 +422,10 @@ void ConstraintDynamicEffector::UpdateState(uint64_t CurrentSimNanos) } /*! Filtering method to calculate filtered Constraint Force - @param CurrentClock The current simulation time (used for time stamping) */ void ConstraintDynamicEffector::computeFilteredForce(uint64_t CurrentClock) { - double F_t[3]; eigenVector3d2CArray(this->Fc_N,F_t); this->F_mag_t = std::sqrt(pow(F_t[0],2)+pow(F_t[1],2)+pow(F_t[2],2)); @@ -309,27 +439,26 @@ void ConstraintDynamicEffector::computeFilteredForce(uint64_t CurrentClock) } /*! Filtering method to calculate filtered Constraint Torque - @param CurrentClock The current simulation time (used for time stamping) */ void ConstraintDynamicEffector::computeFilteredTorque(uint64_t CurrentClock) { - double T_t1[3]; - eigenVector3d2CArray(this->T_B1,T_t1); - this->T1_mag_t = std::sqrt(pow(T_t1[0],2)+pow(T_t1[1],2)+pow(T_t1[2],2)); - this->T1_filtered_mag_t = this->a*this->T1_filtered_mag_tminus1 + - this->b*this->T1_filtered_mag_tminus2+this->c*this->T1_mag_t+ - this->d*this->T1_mag_tminus1+this->e*this->T1_mag_tminus2; - this->T1_filtered_mag_tminus2 = this->T1_filtered_mag_tminus1; - this->T1_filtered_mag_tminus1 = this->T1_filtered_mag_t; - this->T1_mag_tminus2 = this->T1_mag_tminus1; - this->T1_mag_tminus1 = this->T1_mag_t; - double T_t2[3]; - eigenVector3d2CArray(this->T_B2,T_t2); - this->T2_mag_t = std::sqrt(pow(T_t2[0],2)+pow(T_t2[1],2)+pow(T_t2[2],2)); - this->T2_filtered_mag_t = this->a*this->T2_filtered_mag_tminus1 + this->b*this->T2_filtered_mag_tminus2+this->c*this->T2_mag_t+this->d*this->T2_mag_tminus1+this->e*this->T2_mag_tminus2; - this->T2_filtered_mag_tminus2 = this->T2_filtered_mag_tminus1; - this->T2_filtered_mag_tminus1 = this->T2_filtered_mag_t; - this->T2_mag_tminus2 = this->T2_mag_tminus1; - this->T2_mag_tminus1 = this->T2_mag_t; + double T_t1[3]; + eigenVector3d2CArray(this->T_B1,T_t1); + this->T1_mag_t = std::sqrt(pow(T_t1[0],2)+pow(T_t1[1],2)+pow(T_t1[2],2)); + this->T1_filtered_mag_t = this->a*this->T1_filtered_mag_tminus1 + + this->b*this->T1_filtered_mag_tminus2+this->c*this->T1_mag_t+ + this->d*this->T1_mag_tminus1+this->e*this->T1_mag_tminus2; + this->T1_filtered_mag_tminus2 = this->T1_filtered_mag_tminus1; + this->T1_filtered_mag_tminus1 = this->T1_filtered_mag_t; + this->T1_mag_tminus2 = this->T1_mag_tminus1; + this->T1_mag_tminus1 = this->T1_mag_t; + double T_t2[3]; + eigenVector3d2CArray(this->T_B2,T_t2); + this->T2_mag_t = std::sqrt(pow(T_t2[0],2)+pow(T_t2[1],2)+pow(T_t2[2],2)); + this->T2_filtered_mag_t = this->a*this->T2_filtered_mag_tminus1 + this->b*this->T2_filtered_mag_tminus2+this->c*this->T2_mag_t+this->d*this->T2_mag_tminus1+this->e*this->T2_mag_tminus2; + this->T2_filtered_mag_tminus2 = this->T2_filtered_mag_tminus1; + this->T2_filtered_mag_tminus1 = this->T2_filtered_mag_t; + this->T2_mag_tminus2 = this->T2_mag_tminus1; + this->T2_mag_tminus1 = this->T2_mag_t; } diff --git a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h index d33d5ba06c..2a8f779ce9 100644 --- a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h +++ b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h @@ -33,15 +33,22 @@ #include "architecture/msgPayloadDefC/DeviceStatusMsgPayload.h" #include "architecture/messaging/messaging.h" +/*! Struct containing parent classification variables. */ +struct parentID{ + int idx; //!< index of effector's parent within parent type (ex. hub #1 or state eff #2) + std::string parentType; //!< type of parent attached to (hub or state effector) +}; + /*! @brief constraint dynamic effector class */ class ConstraintDynamicEffector: public SysModel, public DynamicEffector { public: ConstraintDynamicEffector(); ~ConstraintDynamicEffector(); - void Reset(uint64_t CurrentSimNanos); - void linkInStates(DynParamManager& states); - void computeForceTorque(double integTime, double timeStep); - void UpdateState(uint64_t CurrentSimNanos); + void Reset(uint64_t CurrentSimNanos) override; + void linkInStates(DynParamManager& states) override; + void linkInProperties(DynParamManager& properties) override; + void computeForceTorque(double integTime, double timeStep) override; + void UpdateState(uint64_t CurrentSimNanos) override; void writeOutputStateMessage(uint64_t CurrentClock); void computeFilteredForce(uint64_t CurrentClock); void computeFilteredTorque(uint64_t CurrentClock); @@ -69,6 +76,30 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector { void setC_a(double c_a); /** setter for `a,b,s,c,d,e` coefficients of low pass filter */ void setFilter_Data(double wc,double h, double k); + /** setter for `stateNameOfPosition` property */ + void setStateNameOfPosition(std::string value) override; + /** setter for `stateNameOfVelocity` property */ + void setStateNameOfVelocity(std::string value) override; + /** setter for `stateNameOfSigma` property */ + void setStateNameOfSigma(std::string value) override; + /** setter for `stateNameOfOmega` property */ + void setStateNameOfOmega(std::string value) override; + /** setter for `propName_inertialPosition` property */ + void setPropName_inertialPosition(std::string value) override; + /** getter for `propName_inertialPosition` property */ + const std::vector getPropName_inertialPosition() const { return this->propName_inertialPosition; } + /** setter for `propName_inertialVelocity` property */ + void setPropName_inertialVelocity(std::string value) override; + /** getter for `propName_inertialVelocity` property */ + const std::vector getPropName_inertialVelocity() const { return this->propName_inertialVelocity; } + /** setter for `propName_inertialAttitude` property */ + void setPropName_inertialAttitude(std::string value) override; + /** getter for `propName_inertialAttitude` property */ + const std::vector getPropName_inertialAttitude() const { return this->propName_inertialAttitude; } + /** setter for `propName_inertialAngVelocity` property */ + void setPropName_inertialAngVelocity(std::string value) override; + /** getter for `propName_inertialAngVelocity` property */ + const std::vector getPropName_inertialAngVelocity() const { return this->propName_inertialAngVelocity; } /** getter for `r_P2P1_B1Init` initial spacecraft separation */ Eigen::Vector3d getR_P2P1_B1Init() const {return this->r_P2P1_B1Init;}; @@ -97,7 +128,10 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector { // Counters and flags int scInitCounter = 0; //!< counter to kill simulation if more than two spacecraft initialized - int scID = 1; //!< 0,1 alternating spacecraft tracker to output appropriate force/torque + int scID = 1; //!< 0,1 alternating spacecraft toggle to output appropriate force/torque + parentID parent1, parent2; + int hubCounter = 0; + int effectorCounter = 0; // Constraint length and direction Eigen::Vector3d r_P1B1_B1 = Eigen::Vector3d::Zero(); //!< [m] position vector from spacecraft 1 hub to its connection point P1 @@ -142,11 +176,25 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector { double T2_filtered_mag_tminus2 = 0.0; //!< Magnitude of filtered constraint torque on s/c 2 at t-2 time step // Simulation variable pointers + std::vector stateNameOfPosition; //!< state engine name of the parent rigid body inertial position vector + std::vector stateNameOfVelocity; //!< state engine name of the parent rigid body inertial velocity vector + std::vector stateNameOfSigma; //!< state engine name of the parent rigid body inertial attitude + std::vector stateNameOfOmega; //!< state engine name of the parent rigid body inertial angular velocity vector std::vector hubPosition; //!< [m] parent inertial position vector std::vector hubVelocity; //!< [m/s] parent inertial velocity vector std::vector hubSigma; //!< parent attitude Modified Rodrigues Parameters (MRPs) std::vector hubOmega; //!< [rad/s] parent inertial angular velocity vector + // Parent body inertial properties + std::vector propName_inertialPosition; //!< property name of inertialPosition + std::vector propName_inertialVelocity; //!< property name of inertialVelocity + std::vector propName_inertialAttitude; //!< property name of inertialAttitude + std::vector propName_inertialAngVelocity; //!< property name of inertialAngVelocity + std::vector inertialPositionProperty; //!< [m] position relative to inertial frame + std::vector inertialVelocityProperty; //!< [m/s] velocity relative to inertial frame + std::vector inertialAttitudeProperty; //!< attitude relative to inertial frame + std::vector inertialAngVelocityProperty; //!< [rad/s] inertial angular velocity relative to inertial frame + // Constraint violations Eigen::Vector3d psi_N = Eigen::Vector3d::Zero(); //!< [m] direction constraint violation in inertial frame Eigen::Vector3d psiPrime_N = Eigen::Vector3d::Zero(); //!< [m/s] direction rate constraint violation in inertial frame diff --git a/src/simulation/dynamics/extForceTorque/extForceTorque.cpp b/src/simulation/dynamics/extForceTorque/extForceTorque.cpp index 8a88764e38..2431586d34 100755 --- a/src/simulation/dynamics/extForceTorque/extForceTorque.cpp +++ b/src/simulation/dynamics/extForceTorque/extForceTorque.cpp @@ -30,6 +30,9 @@ ExtForceTorque::ExtForceTorque() this->extForce_B.fill(0.0); this->extTorquePntB_B.fill(0.0); + /* this effector can be attached onto a state effector */ + isAttachableToStateEffector = true; + CallCounts = 0; return; } diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp index 8594e86fb2..de29c30aaa 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp @@ -47,6 +47,10 @@ SpinningBodyOneDOFStateEffector::SpinningBodyOneDOFStateEffector() this->nameOfThetaState = "spinningBodyTheta" + std::to_string(SpinningBodyOneDOFStateEffector::effectorID); this->nameOfThetaDotState = "spinningBodyThetaDot" + std::to_string(SpinningBodyOneDOFStateEffector::effectorID); + this->nameOfInertialPositionProperty = "spinningBodyInertialPosition" + std::to_string(SpinningBodyOneDOFStateEffector::effectorID); + this->nameOfInertialVelocityProperty = "spinningBodyInertialVelocity" + std::to_string(SpinningBodyOneDOFStateEffector::effectorID); + this->nameOfInertialAttitudeProperty = "spinningBodyInertialAttitude" + std::to_string(SpinningBodyOneDOFStateEffector::effectorID); + this->nameOfInertialAngVelocityProperty = "spinningBodyInertialAngVelocity" + std::to_string(SpinningBodyOneDOFStateEffector::effectorID); SpinningBodyOneDOFStateEffector::effectorID++; } @@ -91,8 +95,8 @@ void SpinningBodyOneDOFStateEffector::writeOutputStateMessages(uint64_t CurrentC // Logging the S frame is the body frame B of that object eigenVector3d2CArray(this->r_ScN_N, configLogMsg.r_BN_N); eigenVector3d2CArray(this->v_ScN_N, configLogMsg.v_BN_N); - eigenVector3d2CArray(this->sigma_SN, configLogMsg.sigma_BN); - eigenVector3d2CArray(this->omega_SN_S, configLogMsg.omega_BN_B); + eigenMatrixXd2CArray(*this->sigma_SN, configLogMsg.sigma_BN); + eigenMatrixXd2CArray(*this->omega_SN_S, configLogMsg.omega_BN_B); this->spinningBodyConfigLogOutMsg.write(&configLogMsg, this->moduleID, CurrentClock); } } @@ -126,6 +130,34 @@ void SpinningBodyOneDOFStateEffector::registerStates(DynParamManager& states) Eigen::MatrixXd thetaDotInitMatrix(1,1); thetaDotInitMatrix(0,0) = this->thetaDotInit; this->thetaDotState->setState(thetaDotInitMatrix); + + registerProperties(states); +} + +void SpinningBodyOneDOFStateEffector::addDynamicEffector(DynamicEffector *newDynamicEffector, int segment = 1) +{ + if (segment != 1) { + bskLogger.bskLog(BSK_ERROR, "Specifying attachment to a non-existent spinning bodies linkage."); + } + + this->assignStateParamNames(newDynamicEffector); + + this->dynEffectors.push_back(newDynamicEffector); +} + +void SpinningBodyOneDOFStateEffector::registerProperties(DynParamManager& states) +{ + Eigen::Vector3d stateInit = Eigen::Vector3d::Zero(); + this->r_SN_N = states.createProperty(this->nameOfInertialPositionProperty, stateInit); + this->v_SN_N = states.createProperty(this->nameOfInertialVelocityProperty, stateInit); + this->sigma_SN = states.createProperty(this->nameOfInertialAttitudeProperty, stateInit); + this->omega_SN_S = states.createProperty(this->nameOfInertialAngVelocityProperty, stateInit); + + std::vector::iterator dynIt; + for(dynIt = this->dynEffectors.begin(); dynIt != this->dynEffectors.end(); dynIt++) + { + (*dynIt)->linkInProperties(states); + } } /*! This method allows the SB state effector to provide its contributions to the mass props and mass prop rates of the @@ -209,6 +241,18 @@ void SpinningBodyOneDOFStateEffector::updateContributions(double integTime, // Define auxiliary variable mTheta this->mTheta = this->sHat_B.transpose() * IPntS_B * this->sHat_B; + // Loop through to collect forces and torques from any connected dynamic effectors + Eigen::Vector3d attBodyForce_S = Eigen::Vector3d::Zero(); + Eigen::Vector3d attBodyTorquePntS_S = Eigen::Vector3d::Zero(); + std::vector::iterator dynIt; + for(dynIt = this->dynEffectors.begin(); dynIt != this->dynEffectors.end(); dynIt++) + { + // - Compute the force and torque contributions from the dynamicEffectors + (*dynIt)->computeForceTorque(integTime, double(0.0)); + attBodyForce_S += (*dynIt)->forceExternal_B; + attBodyTorquePntS_S += (*dynIt)->torqueExternalPntB_B; + } + // Lock the axis if the flag is set to 1 if (this->lockFlag == 1) { @@ -226,12 +270,12 @@ void SpinningBodyOneDOFStateEffector::updateContributions(double integTime, this->bTheta = -(IPntS_B - this->mass * rTilde_SB_B * rTilde_ScS_B) * this->sHat_B / this->mTheta; // Define cTheta - Eigen::Vector3d rDot_SB_B = this->omegaTilde_BN_B * this->r_SB_B; + this->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; + - IPntS_B * this->omegaTilde_BN_B * this->omega_SB_B + this->dcm_BS * attBodyTorquePntS_S + - this->mass * rTilde_ScS_B * this->omegaTilde_BN_B * this->rDot_SB_B)) / this->mTheta; } // For documentation on contributions see Vaz Carneiro, Allard, Schaub spinning body paper @@ -239,7 +283,7 @@ 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 + this->dcm_BS * attBodyForce_S; // Rotation contributions backSubContr.matrixC = (this->IPntSc_B - this->mass * this->rTilde_ScB_B * rTilde_ScS_B) @@ -249,7 +293,8 @@ void SpinningBodyOneDOFStateEffector::updateContributions(double integTime, 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->IPntSc_B - this->mass * this->rTilde_ScB_B * rTilde_ScS_B) * this->sHat_B * this->cTheta + + this->dcm_BS * attBodyTorquePntS_S + eigenTilde(this->r_SB_B) * (this->dcm_BS * attBodyForce_S); } /*! This method is used to find the derivatives for the SB stateEffector: thetaDDot and the kinematic derivative */ @@ -308,13 +353,16 @@ void SpinningBodyOneDOFStateEffector::computeSpinningBodyInertialStates() // inertial attitude Eigen::Matrix3d dcm_SN; dcm_SN = (this->dcm_BS).transpose() * this->dcm_BN; - this->sigma_SN = eigenMRPd2Vector3d(eigenC2MRP(dcm_SN)); + *this->sigma_SN = eigenMRPd2Vector3d(eigenC2MRP(dcm_SN)); + *this->omega_SN_S = (this->dcm_BS).transpose() * this->omega_SN_B; // inertial position vector this->r_ScN_N = (Eigen::Vector3d)*this->inertialPositionProperty + this->dcm_BN.transpose() * this->r_ScB_B; + *this->r_SN_N = (Eigen::Vector3d)(*this->inertialPositionProperty) + this->dcm_BN.transpose() * this->r_SB_B; // inertial velocity vector this->v_ScN_N = (Eigen::Vector3d)*this->inertialVelocityProperty + this->dcm_BN.transpose() * this->rDot_ScB_B; + *this->v_SN_N = (Eigen::Vector3d)*this->inertialVelocityProperty + this->dcm_BN.transpose() * this->rDot_SB_B; } /*! This method is used so that the simulation will ask SB to update messages */ diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h index 7525b54fa4..18c7df8ee0 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h @@ -22,6 +22,7 @@ #include #include "simulation/dynamics/_GeneralModuleFiles/stateEffector.h" +#include "simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h" #include "simulation/dynamics/_GeneralModuleFiles/stateData.h" #include "architecture/_GeneralModuleFiles/sys_model.h" #include "architecture/utilities/avsEigenMRP.h" @@ -45,6 +46,10 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel { double thetaDotInit = 0.0; //!< [rad/s] initial spinning body angle rate std::string nameOfThetaState; //!< -- identifier for the theta state data container std::string nameOfThetaDotState; //!< -- identifier for the thetaDot state data container + std::string nameOfInertialPositionProperty; //!< -- identifier for the inertial position property + std::string nameOfInertialVelocityProperty; //!< -- identifier for the inertial velocity property + std::string nameOfInertialAttitudeProperty; //!< -- identifier for the inertial attitude property + std::string nameOfInertialAngVelocityProperty; //!< -- identifier for the inertial angular velocity property Eigen::Vector3d r_SB_B{0.0, 0.0, 0.0}; //!< [m] vector pointing from body frame B origin to spinning frame S origin in B frame components Eigen::Vector3d r_ScS_S{0.0, 0.0, 0.0}; //!< [m] vector pointing from spinning frame S origin to point Sc (center of mass of the spinner) in S frame components Eigen::Vector3d sHat_S{1.0, 0.0, 0.0}; //!< -- spinning axis in S frame components. @@ -55,6 +60,7 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel { ReadFunctor motorTorqueInMsg; //!< -- (optional) motor torque input message ReadFunctor motorLockInMsg; //!< -- (optional) motor lock flag input message ReadFunctor spinningBodyRefInMsg; //!< -- (optional) spinning body reference input message name + std::vector dynEffectors; //!< Vector of dynamic effectors attached SpinningBodyOneDOFStateEffector(); //!< -- Contructor ~SpinningBodyOneDOFStateEffector() override; //!< -- Destructor @@ -63,6 +69,8 @@ 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 addDynamicEffector(DynamicEffector *newDynamicEffector, int segment) override; //!< -- Method for adding attached dynamic effector + void registerProperties(DynParamManager& states) override; //!< -- Method for registering the SB inertial 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 @@ -81,6 +89,15 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel { double thetaRef = 0.0; //!< [rad] spinning body reference angle double thetaDotRef = 0.0; //!< [rad] spinning body reference angle rate + template + /** Assign the state engine parameter names */ + void assignStateParamNames(Type effector) { + effector->setPropName_inertialPosition(this->nameOfInertialPositionProperty); + effector->setPropName_inertialVelocity(this->nameOfInertialVelocityProperty); + effector->setPropName_inertialAttitude(this->nameOfInertialAttitudeProperty); + effector->setPropName_inertialAngVelocity(this->nameOfInertialAngVelocityProperty); + }; + // Terms needed for back substitution Eigen::Vector3d aTheta{0.0, 0.0, 0.0}; //!< -- rDDot_BN term for back substitution Eigen::Vector3d bTheta{0.0, 0.0, 0.0}; //!< -- omegaDot_BN term for back substitution @@ -93,6 +110,7 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel { Eigen::Vector3d r_ScB_B{0.0, 0.0, 0.0}; //!< [m] vector pointing from body frame B origin to point Sc in B frame components. Eigen::Vector3d rPrime_ScS_B{0.0, 0.0, 0.0}; //!< [m/s] body frame time derivative of r_ScS_B Eigen::Vector3d rPrime_ScB_B{0.0, 0.0, 0.0}; //!< [m/s] body frame time derivative of r_ScB_B + Eigen::Vector3d rDot_SB_B{0.0, 0.0, 0.0}; //!< [m/s] inertial frame time derivative of r_SB_B Eigen::Vector3d rDot_ScB_B{0.0, 0.0, 0.0}; //!< [m/s] inertial frame time derivative of r_ScB_B Eigen::Vector3d omega_SB_B{0.0, 0.0, 0.0}; //!< [rad/s] angular velocity of the S frame wrt the B frame in B frame components. Eigen::Vector3d omega_BN_B{0.0, 0.0, 0.0}; //!< [rad/s] angular velocity of the B frame wrt the N frame in B frame components. @@ -110,16 +128,18 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel { // Spinning body properties Eigen::Vector3d r_ScN_N{0.0, 0.0, 0.0}; //!< [m] position vector of spinning body center of mass Sc relative to the inertial frame origin N Eigen::Vector3d v_ScN_N{0.0, 0.0, 0.0}; //!< [m/s] inertial velocity vector of Sc relative to inertial frame - Eigen::Vector3d sigma_SN{0.0, 0.0, 0.0}; //!< -- MRP attitude of frame S relative to inertial frame - Eigen::Vector3d omega_SN_S{0.0, 0.0, 0.0}; //!< [rad/s] inertial spinning body frame angular velocity vector + Eigen::MatrixXd* r_SN_N; //!< [m] position vector of spinning body origin S relative to the inertial frame origin N + Eigen::MatrixXd* v_SN_N; //!< [m/s] inertial velocity vector of S relative to inertial frame + Eigen::MatrixXd* sigma_SN; //!< -- MRP attitude of frame S relative to inertial frame + Eigen::MatrixXd* omega_SN_S; //!< [rad/s] inertial spinning body frame angular velocity vector // States 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 - StateData *thetaState = nullptr; //!< -- state manager of theta for spinning body - StateData *thetaDotState = nullptr; //!< -- state manager of thetaDot for spinning body + StateData* thetaState = nullptr; //!< -- state manager of theta for spinning body + StateData* thetaDotState = nullptr; //!< -- state manager of thetaDot for spinning body }; diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp index 3fa2419be3..c9e38791a9 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp @@ -56,6 +56,14 @@ SpinningBodyTwoDOFStateEffector::SpinningBodyTwoDOFStateEffector() this->nameOfTheta1DotState = "spinningBodyTheta1Dot" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); this->nameOfTheta2State = "spinningBodyTheta2" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); this->nameOfTheta2DotState = "spinningBodyTheta2Dot" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialPositionProperty1 = "spinningBodyInertialPosition1" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialVelocityProperty1 = "spinningBodyInertialVelocity1" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialAttitudeProperty1 = "spinningBodyInertialAttitude1" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialAngVelocityProperty1 = "spinningBodyInertialAngVelocity1" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialPositionProperty2 = "spinningBodyInertialPosition2" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialVelocityProperty2 = "spinningBodyInertialVelocity2" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialAttitudeProperty2 = "spinningBodyInertialAttitude2" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialAngVelocityProperty2 = "spinningBodyInertialAngVelocity2" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); SpinningBodyTwoDOFStateEffector::effectorID++; } @@ -111,10 +119,10 @@ void SpinningBodyTwoDOFStateEffector::writeOutputStateMessages(uint64_t CurrentC configLogMsg = this->spinningBodyConfigLogOutMsgs[0]->zeroMsgPayload; // Logging the S frame is the body frame B of that object - eigenVector3d2CArray(this->r_Sc1N_N, configLogMsg.r_BN_N); - eigenVector3d2CArray(this->v_Sc1N_N, configLogMsg.v_BN_N); - eigenVector3d2CArray(this->sigma_S1N, configLogMsg.sigma_BN); - eigenVector3d2CArray(this->omega_S1N_S1, configLogMsg.omega_BN_B); + eigenVector3d2CArray((this->r_Sc1N_N), configLogMsg.r_BN_N); + eigenVector3d2CArray((this->v_Sc1N_N), configLogMsg.v_BN_N); + eigenMatrixXd2CArray((*this->sigma_S1N), configLogMsg.sigma_BN); + eigenMatrixXd2CArray((*this->omega_S1N_S1), configLogMsg.omega_BN_B); this->spinningBodyConfigLogOutMsgs[0]->write(&configLogMsg, this->moduleID, CurrentClock); } @@ -125,8 +133,8 @@ void SpinningBodyTwoDOFStateEffector::writeOutputStateMessages(uint64_t CurrentC // Logging the S frame is the body frame B of that object eigenVector3d2CArray(this->r_Sc2N_N, configLogMsg.r_BN_N); eigenVector3d2CArray(this->v_Sc2N_N, configLogMsg.v_BN_N); - eigenVector3d2CArray(this->sigma_S2N, configLogMsg.sigma_BN); - eigenVector3d2CArray(this->omega_S2N_S2, configLogMsg.omega_BN_B); + eigenMatrixXd2CArray(*this->sigma_S2N, configLogMsg.sigma_BN); + eigenMatrixXd2CArray(*this->omega_S2N_S2, configLogMsg.omega_BN_B); this->spinningBodyConfigLogOutMsgs[1]->write(&configLogMsg, this->moduleID, CurrentClock); } } @@ -167,6 +175,35 @@ void SpinningBodyTwoDOFStateEffector::registerStates(DynParamManager& states) thetaDotInitMatrix(1,0) = this->theta2DotInit; this->theta1DotState->setState(thetaDotInitMatrix.row(0)); this->theta2DotState->setState(thetaDotInitMatrix.row(1)); + + registerProperties(states); +} + +void SpinningBodyTwoDOFStateEffector::addDynamicEffector(DynamicEffector *newDynamicEffector, int segment) +{ + this->assignStateParamNames(newDynamicEffector, segment); + + this->dynEffectors.push_back(newDynamicEffector); +} + +void SpinningBodyTwoDOFStateEffector::registerProperties(DynParamManager& states) +{ + Eigen::Vector3d stateInit = Eigen::Vector3d::Zero(); + this->r_S1N_N = states.createProperty(this->nameOfInertialPositionProperty1, stateInit); + this->v_S1N_N = states.createProperty(this->nameOfInertialVelocityProperty1, stateInit); + this->sigma_S1N = states.createProperty(this->nameOfInertialAttitudeProperty1, stateInit); + this->omega_S1N_S1 = states.createProperty(this->nameOfInertialAngVelocityProperty1, stateInit); + + this->r_S2N_N = states.createProperty(this->nameOfInertialPositionProperty2, stateInit); + this->v_S2N_N = states.createProperty(this->nameOfInertialVelocityProperty2, stateInit); + this->sigma_S2N = states.createProperty(this->nameOfInertialAttitudeProperty2, stateInit); + this->omega_S2N_S2 = states.createProperty(this->nameOfInertialAngVelocityProperty2, stateInit); + + std::vector::iterator dynIt; + for(dynIt = this->dynEffectors.begin(); dynIt != this->dynEffectors.end(); dynIt++) + { + (*dynIt)->linkInProperties(states); + } } /*! This method allows the SB state effector to provide its contributions to the mass props and mass prop rates of the @@ -245,6 +282,10 @@ void SpinningBodyTwoDOFStateEffector::updateEffectorMassProps(double integTime) this->rPrime_ScB_B = (this->mass1 * this->rPrime_Sc1B_B + this->mass2 * this->rPrime_Sc2B_B) / this->mass; this->effProps.rEffPrime_CB_B = this->rPrime_ScB_B; + // Compute the effector's velocity properties + this->rDot_S1B_B = this->omegaTilde_BN_B * this->r_S1B_B; + this->rDot_S2B_B = this->rDot_S1B_B + this->rPrime_S2S1_B + this->omegaTilde_BN_B * this->r_S2S1_B; + // Find the body-frame time derivative of the inertias of each spinner this->IPrimeS1PntSc1_B = this->omegaTilde_S1B_B * this->IS1PntSc1_B - this->IS1PntSc1_B * this->omegaTilde_S1B_B; this->IPrimeS2PntSc2_B = this->omegaTilde_S2B_B * this->IS2PntSc2_B - this->IS2PntSc2_B * this->omegaTilde_S2B_B; @@ -271,6 +312,26 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back gLocal_N = g_N; g_B = this->dcm_BN * gLocal_N; + // Loop through to collect forces and torques from any connected dynamic effectors + Eigen::Vector3d attBodyForce_S1 = Eigen::Vector3d::Zero(); + Eigen::Vector3d attBodyTorquePntS1_S1 = Eigen::Vector3d::Zero(); + Eigen::Vector3d attBodyForce_S2 = Eigen::Vector3d::Zero(); + Eigen::Vector3d attBodyTorquePntS2_S2 = Eigen::Vector3d::Zero(); + std::vector::iterator dynIt; + for(dynIt = this->dynEffectors.begin(); dynIt != this->dynEffectors.end(); dynIt++) + { + // - Compute the force and torque contributions from the dynamicEffectors + (*dynIt)->computeForceTorque(integTime, double(0.0)); + if ((*dynIt)->getPropName_inertialPosition() == this->nameOfInertialPositionProperty1) { + attBodyForce_S1 += (*dynIt)->forceExternal_B; + attBodyTorquePntS1_S1 += (*dynIt)->torqueExternalPntB_B; + } + else if ((*dynIt)->getPropName_inertialPosition() == this->nameOfInertialPositionProperty2) { + attBodyForce_S2 += (*dynIt)->forceExternal_B; + attBodyTorquePntS2_S2 += (*dynIt)->torqueExternalPntB_B; + } + } + // Update omega_BN_B this->omega_BN_B = omega_BN_B; this->omegaTilde_BN_B = eigenTilde(this->omega_BN_B); @@ -334,12 +395,15 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back + this->mass1 * (rTilde_Sc1S1_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * rTilde_Sc1S1_B) * this->rPrime_Sc1S1_B + this->mass2 * (rTilde_Sc2S1_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * rTilde_Sc2S1_B) * this->rPrime_Sc2S1_B + this->mass2 * rTilde_Sc2S1_B * omegaTilde_S2S1_B * this->rPrime_Sc2S2_B - + this->mass * rTilde_ScS1_B * this->omegaTilde_BN_B * rDot_S1B_B); + + this->mass * rTilde_ScS1_B * this->omegaTilde_BN_B * rDot_S1B_B + - this->dcm_BS1 * attBodyTorquePntS1_S1 + this->dcm_BS2 * attBodyTorquePntS2_S2 + - eigenTilde(this->r_S2S1_B) * (this->dcm_BS2 * attBodyForce_S2)); CThetaStar(1, 0) = this->u2 - this->k2 * (this->theta2 - this->theta2Ref) - this->c2 * (this->theta2Dot - this->theta2DotRef) + this->s2Hat_B.transpose() * gravityTorquePntS2_B - this->s2Hat_B.transpose() * ((IPrimeS2PntS2_B + this->omegaTilde_BN_B * IS2PntS2_B) * this->omega_S2N_B + IS2PntS2_B * this->omegaTilde_S1B_B * this->omega_S2S1_B + this->mass2 * rTilde_Sc2S2_B * omegaTilde_S1N_B * this->rPrime_S2S1_B - + this->mass2 * rTilde_Sc2S2_B * this->omegaTilde_BN_B * (rDot_S2S1_B + rDot_S1B_B)); + + this->mass2 * rTilde_Sc2S2_B * this->omegaTilde_BN_B * (rDot_S2S1_B + rDot_S1B_B) + - this->dcm_BS2 * attBodyTorquePntS2_S2); // Check if any of the axis are locked and change dynamics accordingly if (this->lockFlag1 == 1 || this->lockFlag2 == 1) @@ -373,7 +437,8 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back backSubContr.matrixA = - this->mass * rTilde_ScS1_B * this->s1Hat_B * this->ATheta.row(0) - this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->ATheta.row(1); backSubContr.matrixB = - this->mass * rTilde_ScS1_B * this->s1Hat_B * this->BTheta.row(0) - this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->BTheta.row(1); backSubContr.vecTrans = - this->mass * this->omegaTilde_S1B_B * this->rPrime_ScB_B - this->mass2 * (omegaTilde_S2S1_B * this->rPrime_Sc2S2_B - rTilde_Sc2S2_B * this->omegaTilde_S1B_B * this->omega_S2S1_B) - + this->mass * rTilde_ScS1_B * this->s1Hat_B * this->CTheta.row(0) + this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->CTheta.row(1); + + this->mass * rTilde_ScS1_B * this->s1Hat_B * this->CTheta.row(0) + this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->CTheta.row(1) + + this->dcm_BS1 * attBodyForce_S1 + this->dcm_BS2 * attBodyForce_S2; // Rotation contributions backSubContr.matrixC = (this->IS1PntSc1_B + this->IS2PntSc2_B - this->mass1 * this->rTilde_Sc1B_B * rTilde_Sc1S1_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S1_B) * this->s1Hat_B * this->ATheta.row(0) @@ -386,7 +451,10 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back - this->mass2 * (this->rTilde_Sc2B_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * this->rTilde_Sc2B_B) * this->rPrime_Sc2B_B - this->mass2 * this->rTilde_Sc2B_B * omegaTilde_S2S1_B * this->rPrime_Sc2S2_B - (this->IS1PntSc1_B + this->IS2PntSc2_B - this->mass1 * this->rTilde_Sc1B_B * rTilde_Sc1S1_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S1_B) * this->s1Hat_B * this->CTheta.row(0) - - (this->IS2PntSc2_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S2_B) * this->s2Hat_B * this->CTheta.row(1); + - (this->IS2PntSc2_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S2_B) * this->s2Hat_B * this->CTheta.row(1) + + this->dcm_BS1 * attBodyTorquePntS1_S1 + this->dcm_BS2 * attBodyTorquePntS2_S2 + + eigenTilde(this->r_S1B_B) * (this->dcm_BS1 * attBodyForce_S1) + + eigenTilde(this->r_S2S1_B + this->r_S1B_B) * (this->dcm_BS2 * attBodyForce_S2); } /*! This method is used to find the derivatives for the SB stateEffector: thetaDDot and the kinematic derivative */ @@ -439,20 +507,24 @@ void SpinningBodyTwoDOFStateEffector::computeSpinningBodyInertialStates() Eigen::Matrix3d dcm_S2N; dcm_S1N = (this->dcm_BS1).transpose() * this->dcm_BN; dcm_S2N = (this->dcm_BS2).transpose() * this->dcm_BN; - this->sigma_S1N = eigenMRPd2Vector3d(eigenC2MRP(dcm_S1N)); - this->sigma_S2N = eigenMRPd2Vector3d(eigenC2MRP(dcm_S2N)); + *this->sigma_S1N = eigenMRPd2Vector3d(eigenC2MRP(dcm_S1N)); + *this->sigma_S2N = eigenMRPd2Vector3d(eigenC2MRP(dcm_S2N)); // Convert the angular velocity to the corresponding frame - this->omega_S1N_S1 = dcm_BS1.transpose() * this->omega_S1N_B; - this->omega_S2N_S2 = dcm_BS2.transpose() * this->omega_S2N_B; + *this->omega_S1N_S1 = dcm_BS1.transpose() * this->omega_S1N_B; + *this->omega_S2N_S2 = dcm_BS2.transpose() * this->omega_S2N_B; - // Compute the inertial position vector + // Compute the inertial position vectors this->r_Sc1N_N = (Eigen::Vector3d)(*this->inertialPositionProperty) + this->dcm_BN.transpose() * this->r_Sc1B_B; this->r_Sc2N_N = (Eigen::Vector3d)(*this->inertialPositionProperty) + this->dcm_BN.transpose() * this->r_Sc2B_B; + *this->r_S1N_N = (Eigen::Vector3d)(*this->inertialPositionProperty) + this->dcm_BN.transpose() * this->r_S1B_B; + *this->r_S2N_N = (Eigen::Vector3d)(*this->inertialPositionProperty) + this->dcm_BN.transpose() * (this->r_S1B_B + this->r_S2S1_B); - // Compute the inertial velocity vector + // Compute the inertial velocity vectors this->v_Sc1N_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) + this->dcm_BN.transpose() * this->rDot_Sc1B_B; this->v_Sc2N_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) + this->dcm_BN.transpose() * this->rDot_Sc2B_B; + *this->v_S1N_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) + this->dcm_BN.transpose() * this->rDot_S1B_B; + *this->v_S2N_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) + this->dcm_BN.transpose() * this->rDot_S2B_B; } /*! This method is used so that the simulation will ask SB to update messages */ diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h index e33e3da85d..ba42782913 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h @@ -22,6 +22,7 @@ #include #include "simulation/dynamics/_GeneralModuleFiles/stateEffector.h" +#include "simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h" #include "simulation/dynamics/_GeneralModuleFiles/stateData.h" #include "architecture/_GeneralModuleFiles/sys_model.h" #include "architecture/utilities/avsEigenMRP.h" @@ -51,6 +52,14 @@ class SpinningBodyTwoDOFStateEffector: public StateEffector, public SysModel { std::string nameOfTheta1DotState; //!< -- identifier for the thetaDot1 state data container std::string nameOfTheta2State; //!< -- identifier for the theta2 state data container std::string nameOfTheta2DotState; //!< -- identifier for the thetaDot2 state data container + std::string nameOfInertialPositionProperty1; //!< -- identifier for the lower spinning body inertial position property + std::string nameOfInertialVelocityProperty1; //!< -- identifier for the lower spinning body inertial velocity property + std::string nameOfInertialAttitudeProperty1; //!< -- identifier for the lower spinning body inertial attitude property + std::string nameOfInertialAngVelocityProperty1; //!< -- identifier for the lower spinning body inertial angular velocity property + std::string nameOfInertialPositionProperty2; //!< -- identifier for the upper spinning body inertial position property + std::string nameOfInertialVelocityProperty2; //!< -- identifier for the upper spinning body inertial velocity property + std::string nameOfInertialAttitudeProperty2; //!< -- identifier for the upper spinning body inertial attitude property + std::string nameOfInertialAngVelocityProperty2; //!< -- identifier for the upper spinning body inertial angular velocity property Eigen::Vector3d r_S1B_B{0.0,0.0,0.0}; //!< [m] vector pointing from body frame B origin to lower spinning frame S1 origin in B frame components Eigen::Vector3d r_S2S1_S1{0.0,0.0,0.0}; //!< [m] vector pointing from lower spinning frame S1 origin to upper spinning frame S2 origin in S1 frame components Eigen::Vector3d r_Sc1S1_S1{0.0,0.0,0.0}; //!< [m] vector pointing from lower spinning frame S1 origin to point Sc1 (center of mass of the lower spinner) in S1 frame components @@ -69,29 +78,32 @@ class SpinningBodyTwoDOFStateEffector: public StateEffector, public SysModel { ReadFunctor motorLockInMsg; //!< -- (optional) motor lock input message name std::vector> spinningBodyRefInMsgs {ReadFunctor(), ReadFunctor()}; //!< (optional) vector of spinning body reference input messages + std::vector dynEffectors; //!< Vector of dynamic effectors attached SpinningBodyTwoDOFStateEffector(); //!< -- Contructor ~SpinningBodyTwoDOFStateEffector(); //!< -- Destructor - void Reset(uint64_t CurrentClock); //!< -- Method for reset - void writeOutputStateMessages(uint64_t CurrentClock); //!< -- Method for writing the output messages - 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 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 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 addDynamicEffector(DynamicEffector *newDynamicEffector, int segment) override; //!< -- Method for adding attached dynamic effector + void registerProperties(DynParamManager& states) override; //!< -- Method for registering the SB inertial properties void updateContributions(double integTime, BackSubMatrices& backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, - Eigen::Vector3d g_N); //!< -- Method for back-substitution contributions + Eigen::Vector3d g_N) override; //!< -- Method for back-substitution contributions void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, - Eigen::Vector3d sigma_BN); //!< -- Method for SB to compute its derivatives - void updateEffectorMassProps(double integTime); //!< -- Method for giving the s/c the HRB mass props and prop rates + Eigen::Vector3d sigma_BN) override; //!< -- Method for SB to compute its derivatives + void updateEffectorMassProps(double integTime) override; //!< -- Method for giving the s/c the HRB mass props and prop rates void updateEnergyMomContributions(double integTime, Eigen::Vector3d& rotAngMomPntCContr_B, double& rotEnergyContr, - Eigen::Vector3d omega_BN_B); //!< -- Method for computing energy and momentum for SBs - void prependSpacecraftNameToStates(); //!< Method used for multiple spacecraft + 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 private: @@ -106,6 +118,23 @@ class SpinningBodyTwoDOFStateEffector: public StateEffector, public SysModel { double theta2DotRef = 0.0; //!< [rad] spinning body reference angle rate double mass = 1.0; //!< [kg] mass of the spinner system + template + /** Assign the state engine parameter names */ + void assignStateParamNames(Type effector, int segment) { + if (segment == 1) { + effector->setPropName_inertialPosition(this->nameOfInertialPositionProperty1); + effector->setPropName_inertialVelocity(this->nameOfInertialVelocityProperty1); + effector->setPropName_inertialAttitude(this->nameOfInertialAttitudeProperty1); + effector->setPropName_inertialAngVelocity(this->nameOfInertialAngVelocityProperty1); + } + else if (segment == 2) { + effector->setPropName_inertialPosition(this->nameOfInertialPositionProperty2); + effector->setPropName_inertialVelocity(this->nameOfInertialVelocityProperty2); + effector->setPropName_inertialAttitude(this->nameOfInertialAttitudeProperty2); + effector->setPropName_inertialAngVelocity(this->nameOfInertialAngVelocityProperty2); + } + }; + // Terms needed for back substitution Eigen::Matrix ATheta; //!< -- rDDot_BN term for back substitution Eigen::Matrix BTheta; //!< -- omegaDot_BN term for back substitution @@ -130,6 +159,8 @@ class SpinningBodyTwoDOFStateEffector: public StateEffector, public SysModel { Eigen::Vector3d rPrime_ScB_B{0.0,0.0,0.0}; //!< [m/s] body frame time derivative of r_ScB_B Eigen::Vector3d rDot_Sc1B_B{0.0,0.0,0.0}; //!< [m/s] inertial frame time derivative of r_Sc1B_B Eigen::Vector3d rDot_Sc2B_B{0.0,0.0,0.0}; //!< [m/s] inertial frame time derivative of r_Sc2B_B + Eigen::Vector3d rDot_S1B_B{0.0,0.0,0.0}; //!< [m/s] inertial frame time derivative of r_S1B_B + Eigen::Vector3d rDot_S2B_B{0.0,0.0,0.0}; //!< [m/s] inertial frame time derivative of r_S2B_B Eigen::Vector3d omega_S1B_B{0.0,0.0,0.0}; //!< [rad/s] angular velocity of the S1 frame wrt the B frame in B frame components Eigen::Vector3d omega_S2S1_B{0.0,0.0,0.0}; //!< [rad/s] angular velocity of the S2 frame wrt the S1 frame in B frame components Eigen::Vector3d omega_S2B_B{0.0,0.0,0.0}; //!< [rad/s] angular velocity of the S2 frame wrt the B frame in B frame components @@ -157,10 +188,14 @@ class SpinningBodyTwoDOFStateEffector: public StateEffector, public SysModel { Eigen::Vector3d r_Sc2N_N{0.0,0.0,0.0}; //!< [m] position vector of upper spinning body center of mass Sc2 relative to the inertial frame origin N Eigen::Vector3d v_Sc1N_N{0.0,0.0,0.0}; //!< [m/s] inertial velocity vector of Sc1 relative to inertial frame Eigen::Vector3d v_Sc2N_N{0.0,0.0,0.0}; //!< [m/s] inertial velocity vector of Sc2 relative to inertial frame - Eigen::Vector3d sigma_S1N{0.0,0.0,0.0}; //!< -- MRP attitude of frame S1 relative to inertial frame - Eigen::Vector3d sigma_S2N{0.0,0.0,0.0}; //!< -- MRP attitude of frame S2 relative to inertial frame - Eigen::Vector3d omega_S1N_S1{0.0,0.0,0.0}; //!< [rad/s] inertial lower spinning body frame angular velocity vector - Eigen::Vector3d omega_S2N_S2{0.0,0.0,0.0}; //!< [rad/s] inertial upper spinning body frame angular velocity vector + Eigen::MatrixXd* r_S1N_N; //!< [m] position vector of lower spinning body origin S1 relative to the inertial frame origin N + Eigen::MatrixXd* r_S2N_N; //!< [m] position vector of upper spinning body origin S2 relative to the inertial frame origin N + Eigen::MatrixXd* v_S1N_N; //!< [m/s] inertial velocity vector of S1 relative to inertial frame + Eigen::MatrixXd* v_S2N_N; //!< [m/s] inertial velocity vector of S2 relative to inertial frame + Eigen::MatrixXd* sigma_S1N; //!< -- MRP attitude of frame S1 relative to inertial frame + Eigen::MatrixXd* sigma_S2N; //!< -- MRP attitude of frame S2 relative to inertial frame + Eigen::MatrixXd* omega_S1N_S1; //!< [rad/s] inertial lower spinning body frame angular velocity vector + Eigen::MatrixXd* omega_S2N_S2; //!< [rad/s] inertial upper spinning body frame angular velocity vector // States double theta1 = 0.0; //!< [rad] first axis angle @@ -169,8 +204,8 @@ class SpinningBodyTwoDOFStateEffector: public StateEffector, public SysModel { double theta2Dot = 0.0; //!< [rad/s] second axis 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 - StateData *theta1State = nullptr; //!< -- state manager of theta1 for spinning body - StateData *theta1DotState = nullptr; //!< -- state manager of theta1Dot for spinning body + StateData* theta1State = nullptr; //!< -- state manager of theta1 for spinning body + 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 }; diff --git a/src/utilities/simIncludeThruster.py b/src/utilities/simIncludeThruster.py index 1685c687b2..1daf3106ed 100755 --- a/src/utilities/simIncludeThruster.py +++ b/src/utilities/simIncludeThruster.py @@ -236,6 +236,37 @@ def addToSpacecraft(self, modelTag, thEffector, sc): return + def addToSpacecraftSubcomponent(self, modelTag, thEffector, baseEffector, segment=0): + """ + This function is for adding a thruster cluster to intermediate bodies + + Parameters + ---------- + modelTag: string + module model tag string + thEffector: thrusterEffector + thruster effector handle + baseEffector: intermediate body to be attached to, see table for compatibility guide + segment: if subcomponent is multi-body, designate which integer segment + default segment to base segment + """ + + thEffector.ModelTag = modelTag + + for key, th in list(self.thrusterList.items()): + thEffector.addThruster(th) + + # Check the type of thruster effector + thrusterType = str(type(thEffector)) + if 'ThrusterDynamicEffector' in thrusterType: + baseEffector.addDynamicEffector(thEffector, segment) + elif 'ThrusterStateEffector' in thrusterType: + baseEffector.addStateEffector(thEffector, segment) + else: + print("This isn't a thruster effector. You did something wrong.") + + return + def getNumOfDevices(self): """ Returns the number of RW devices setup.