Enable facet drag dynamic effector to link to state effectors

docs/source/Learn/bskPrinciples/bskPrinciples-11.rst

@@ -164,6 +164,7 @@ special case of prescribed effector branching explained in :ref:`prescribedMotio
           <th class="rotate"><span class="rotate-inner">Thruster State Effector</span></th>
           <th class="rotate"><span class="rotate-inner">Constraint Effector</span></th>
           <th class="rotate"><span class="rotate-inner">Drag Effector</span></th>
+          <th class="rotate"><span class="rotate-inner">Facet Drag Effector</span></th>
           <th class="rotate"><span class="rotate-inner">External Force Torque</span></th>
           <th class="rotate"><span class="rotate-inner">SRP Effector</span></th>
           <th class="rotate"><span class="rotate-inner">Fuel Tank Effector</span></th>
@@ -185,63 +186,63 @@ special case of prescribed effector branching explained in :ref:`prescribedMotio
             <span class="rotate-inner">Parent Effectors</span>
           </td>
           <td class="label">Spinning Bodies 1DOF</td>
-          <td class="green"></td><td class="yellow"></td><td class="green"></td><td class="yellow"></td>
+          <td class="green"></td><td class="yellow"></td><td class="green"></td><td class="yellow"></td><td class="green"></td>
           <td class="green"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
         </tr>
         <tr>
           <td class="label">Spinning Bodies 2DOF</td>
-          <td class="green"></td><td class="yellow"></td><td class="green"></td><td class="yellow"></td>
+          <td class="green"></td><td class="yellow"></td><td class="green"></td><td class="yellow"></td><td class="green"></td>
           <td class="green"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
         </tr>
         <tr>
           <td class="label">Spinning Bodies NDOF</td>
-          <td class="green"></td><td class="yellow"></td><td class="green"></td><td class="yellow"></td>
+          <td class="green"></td><td class="yellow"></td><td class="green"></td><td class="yellow"></td><td class="green"></td>
           <td class="green"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
         </tr>
         <tr>
           <td class="label">Hinged Rigid Bodies</td>
-          <td class="green"></td><td class="yellow"></td><td class="green"></td><td class="yellow"></td>
+          <td class="green"></td><td class="yellow"></td><td class="green"></td><td class="yellow"></td><td class="green"></td>
           <td class="green"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
         </tr>
         <tr>
           <td class="label">Dual Hinged Rigid Bodies</td>
-          <td class="yellow"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td>
+          <td class="yellow"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td>
           <td class="yellow"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
         </tr>
         <tr>
           <td class="label">N Hinged Rigid Bodies</td>
-          <td class="yellow"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td>
+          <td class="yellow"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td>
           <td class="yellow"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
         </tr>
         <tr>
           <td class="label">Linear Translating Bodies 1DOF</td>
-          <td class="green"></td><td class="yellow"></td><td class="green"></td><td class="yellow"></td>
+          <td class="green"></td><td class="yellow"></td><td class="green"></td><td class="yellow"></td><td class="green"></td>
           <td class="green"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
         </tr>
         <tr>
           <td class="label">Linear Translating Bodies NDOF</td>
-          <td class="yellow"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td>
+          <td class="yellow"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td>
           <td class="yellow"></td><td class="yellow"></td><td class="yellow"></td><td class="yellow"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
           <td class="red"></td><td class="red"></td><td class="red"></td><td class="red"></td>
         </tr>
         <tr>
           <td class="label">Prescribed Motion</td>
-          <td class="blue"></td><td class="blue"></td><td class="blue"></td><td class="blue"></td>
+          <td class="blue"></td><td class="blue"></td><td class="blue"></td><td class="blue"></td><td class="blue"></td>
           <td class="blue"></td><td class="blue"></td><td class="blue"></td><td class="blue"></td>
           <td class="blue"></td><td class="blue"></td><td class="blue"></td><td class="blue"></td>
           <td class="blue"></td><td class="blue"></td><td class="blue"></td><td class="blue"></td>

docs/source/Support/bskReleaseNotesSnippets/1289-faceted-drag.rst

@@ -0,0 +1 @@
+- Added support for :ref:`FacetDragDynamicEffector` to attach to state effectors.

src/simulation/dynamics/_GeneralModuleFiles/_UnitTestDynamics/test_effectorBranching_integrated.py

@@ -32,6 +32,10 @@
 path = os.path.dirname(os.path.abspath(filename))
 splitPath = path.split('simulation')
 
+from Basilisk import __path__
+bskPath = __path__[0]
+
+
 from Basilisk.utilities import SimulationBaseClass, macros, simIncludeThruster
 from Basilisk.utilities import RigidBodyKinematics as rbk
 from Basilisk.architecture.bskLogging import BasiliskError
@@ -60,11 +64,15 @@
     thrusterDynamicEffector,
     constraintDynamicEffector,
     dragDynamicEffector,
+    facetDragDynamicEffector,
     radiationPressure,
     facetSRPDynamicEffector,
     MtbEffector,
 )
 from Basilisk.architecture import messaging
+from Basilisk.simulation import tabularAtmosphere
+from Basilisk.utilities.readAtmTable import readAtmTable
+from Basilisk.simulation import simpleNav
 
 # uncomment this line if this test is to be skipped in the global unit test run, adjust message as needed
 # @pytest.mark.skipif(conditionstring)
@@ -97,6 +105,7 @@
     ("constraintEffectorOneHub",  True),
     ("constraintEffectorNoHubs",  True),
     # ("dragEffector",                False),
+    ("facetDragDynamicEffector",  True),
     # ("radiationPressure",           False),
     # ("facetSRPDynamicEffector",     False),
     # ("MtbEffector",                 False),
@@ -272,6 +281,28 @@ def effectorBranchingIntegratedTest(show_plots, stateEffector, isParent, dynamic
         dynamicEff = setup_extPulseTorque()
     elif dynamicEffector == "thrusterDynamicEffector":
         dynamicEff, thFactory = setup_thrusterDynamicEffector()
+    elif dynamicEffector == "facetDragDynamicEffector":
+        dynamicEff = setup_facetDragDynamicEffector(stateEffProps)
+
+        tabAtmo = tabularAtmosphere.TabularAtmosphere()
+        tabAtmo.ModelTag = "tabularAtmosphere"
+
+        tabAtmo.addSpacecraftToModel(scObject.scStateOutMsg)
+
+        r_eq = 6378136.6
+        dataFileName = bskPath + '/supportData/AtmosphereData/EarthGRAMNominal.txt'
+        altList, rhoList, tempList = readAtmTable(dataFileName, 'EarthGRAM')
+
+        # assign constants & ref. data to module
+        tabAtmo.planetRadius = r_eq
+        tabAtmo.altList = tabularAtmosphere.DoubleVector(altList)
+        tabAtmo.rhoList = tabularAtmosphere.DoubleVector(rhoList)
+        tabAtmo.tempList = tabularAtmosphere.DoubleVector(tempList)
+
+        dynamicEff.atmoDensInMsg.subscribeTo(tabAtmo.envOutMsgs[0])
+
+        unitTestSim.AddModelToTask(unitTaskName, tabAtmo)
+
     elif dynamicEffector == "constraintEffectorOneHub":
         dynamicEff, scObjectx = setup_constraintEffectorOneHub(scObject, stateEffProps)
         unitTestSim.AddModelToTask("unitTask", scObjectx)
@@ -541,6 +572,33 @@ def setup_thrusterDynamicEffector():
 
     return(thruster, thFactory)
 
+def setup_facetDragDynamicEffector(stateEffProps):
+    facetDrag = facetDragDynamicEffector.FacetDragDynamicEffector()
+    facetDrag.ModelTag = "facetDragDynamicEffector"
+
+    facetDrag.setPropName_inertialVelocity(stateEffProps.nameOfInertialVelocityProperty)
+    facetDrag.setPropName_inertialAttitude(stateEffProps.nameOfInertialAttitudeProperty)
+
+    panelArea = 10
+    panelOffset = 0.3  # Distance from hub COM
+    panelCd = 5
+
+    panel_normals = [
+        np.array([0, 0,  1]),
+        np.array([0, 0, -1])
+    ]
+
+    # Centered on panel
+    panel_locations = [
+        np.array([0.0, 0.0, panelOffset]),
+        np.array([0.0, 0.0, panelOffset])
+    ]
+
+    for i in range(2):
+        facetDrag.addFacet(panelArea, panelCd, panel_normals[i], panel_locations[i])
+
+    return(facetDrag)
+
 def setup_constraintEffector(scObject1):
     scObject2 = spacecraft.Spacecraft()
     scObject2.ModelTag = "spacecraftBody2"
@@ -670,6 +728,9 @@ def setup_spinningBodiesOneDOF():
     stateEffProps.r_PB_B = spinningBody.r_SB_B
     stateEffProps.r_PcP_P = spinningBody.r_ScS_S
     stateEffProps.inertialPropLogName = "spinningBodyConfigLogOutMsg"
+    stateEffProps.nameOfInertialPositionProperty = spinningBody.ModelTag + "InertialPosition1"
+    stateEffProps.nameOfInertialVelocityProperty = spinningBody.ModelTag + "InertialVelocity1"
+    stateEffProps.nameOfInertialAttitudeProperty = spinningBody.ModelTag + "InertialAttitude1"
 
     return(spinningBody, stateEffProps)
 
@@ -710,6 +771,9 @@ def setup_spinningBodiesTwoDOF():
     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"
+    stateEffProps.nameOfInertialPositionProperty = spinningBody.ModelTag + "InertialPosition1"
+    stateEffProps.nameOfInertialVelocityProperty = spinningBody.ModelTag + "InertialVelocity1"
+    stateEffProps.nameOfInertialAttitudeProperty = spinningBody.ModelTag + "InertialAttitude1"
 
     return(spinningBody, stateEffProps)
 
@@ -778,6 +842,9 @@ def setup_spinningBodiesNDOF():
     stateEffProps.r_PB_B = r_ScB_B - dcm_SB.transpose() @ spinningBody.getR_ScS_S()
     stateEffProps.r_PcP_P = spinningBody.getR_ScS_S()
     stateEffProps.inertialPropLogName = "spinningBodyConfigLogOutMsgs"
+    stateEffProps.nameOfInertialPositionProperty = spinningBodyEffector.ModelTag + "InertialPosition1"
+    stateEffProps.nameOfInertialVelocityProperty = spinningBodyEffector.ModelTag + "InertialVelocity1"
+    stateEffProps.nameOfInertialAttitudeProperty = spinningBodyEffector.ModelTag + "InertialAttitude1"
 
     return(spinningBodyEffector, stateEffProps)
 
@@ -808,6 +875,9 @@ def setup_hingedRigidBodyStateEffector():
     stateEffProps.r_PB_B = hingedBody.r_HB_B
     stateEffProps.r_PcP_P = hingedBody.d * s1_hat
     stateEffProps.inertialPropLogName = "hingedRigidBodyConfigLogOutMsg"
+    stateEffProps.nameOfInertialPositionProperty = hingedBody.ModelTag + "InertialPosition1"
+    stateEffProps.nameOfInertialVelocityProperty = hingedBody.ModelTag + "InertialVelocity1"
+    stateEffProps.nameOfInertialAttitudeProperty = hingedBody.ModelTag + "InertialAttitude1"
 
     return(hingedBody, stateEffProps)
 
@@ -841,6 +911,9 @@ def setup_translatingBodiesOneDOF():
     stateEffProps.r_PB_B = translatingBody.getR_F0B_B()
     stateEffProps.r_PcP_P = translatingBody.getR_FcF_F()
     stateEffProps.inertialPropLogName = "translatingBodyConfigLogOutMsg"
+    stateEffProps.nameOfInertialPositionProperty = translatingBody.ModelTag + "InertialPosition1"
+    stateEffProps.nameOfInertialVelocityProperty = translatingBody.ModelTag + "InertialVelocity1"
+    stateEffProps.nameOfInertialAttitudeProperty = translatingBody.ModelTag + "InertialAttitude1"
 
     return(translatingBody, stateEffProps)
 
@@ -862,6 +935,9 @@ def setup_linearSpringMassDamper():
     stateEffProps.totalMass = linearSpring.massInit
     stateEffProps.mr_PcB_B = mr_ScB_B
     stateEffProps.r_PB_B = linearSpring.r_PB_B
+    stateEffProps.nameOfInertialPositionProperty = linearSpring.ModelTag + "InertialPosition1"
+    stateEffProps.nameOfInertialVelocityProperty = linearSpring.ModelTag + "InertialVelocity1"
+    stateEffProps.nameOfInertialAttitudeProperty = linearSpring.ModelTag + "InertialAttitude1"
 
     return(linearSpring, stateEffProps)
 
@@ -873,6 +949,9 @@ class stateEffectorProperties:
     # 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
+    nameOfInertialPositionProperty = ""
+    nameOfInertialVelocityProperty = ""
+    nameOfInertialAttitudeProperty = ""
 
 if __name__ == "__main__":
-    effectorBranchingIntegratedTest(True, "hingedRigidBodies", True, "extForceTorque", True)
+    effectorBranchingIntegratedTest(True, "hingedRigidBodies", True, "facetDragDynamicEffector", True)

src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.cpp

@@ -29,7 +29,23 @@ FacetDragDynamicEffector::FacetDragDynamicEffector()
     this->v_B.fill(0.0);
     this->v_hat_B.fill(0.0);
 	this->numFacets = 0;
-	return;
+
+   // Initialize state pointers
+   this->hubSigma = nullptr;
+   this->hubVelocity = nullptr;
+
+   // Initialize property pointers
+   this->inertialVelocityProperty = nullptr;
+   this->inertialAttitudeProperty = nullptr;
+
+   // Set default state names for hub attachment
+   this->stateNameOfSigma = "hubSigma";
+   this->stateNameOfVelocity = "hubVelocity";
+
+   /* This effector can be attached onto a state effector */
+   isAttachableToStateEffector = true;
+
+   return;
 }
 
 /*! The destructor.*/
@@ -97,17 +113,59 @@ void FacetDragDynamicEffector::linkInStates(DynParamManager& states){
 	this->hubVelocity = states.getStateObject(this->stateNameOfVelocity);
 }
 
-/*! This method updates the internal drag direction based on the spacecraft velocity vector.
+/*! This method is used to link the dragEffector to the state effectors attitude, position and velocity,
+which are required for calculating drag forces and torques.
+@param properties The parameter manager to collect from
 */
-void FacetDragDynamicEffector::updateDragDir(){
-    Eigen::MRPd sigmaBN;
-    sigmaBN = (Eigen::Vector3d)this->hubSigma->getState();
-    Eigen::Matrix3d dcm_BN = sigmaBN.toRotationMatrix().transpose();
+void FacetDragDynamicEffector::linkInProperties(DynParamManager& properties)
+{
+   // updateDragDir only requires these two to do calculations
+   this->inertialAttitudeProperty = properties.getPropertyReference(this->propName_inertialAttitude);
+   this->inertialVelocityProperty = properties.getPropertyReference(this->propName_inertialVelocity);
+}
 
-    this->v_B = dcm_BN*this->hubVelocity->getState(); // [m/s] sc velocity
-    this->v_hat_B = this->v_B / this->v_B.norm();
+/*! This method is used to set the inertialVelocity property when facet drag is attached to a state effector rather than the hub
+ */
+void FacetDragDynamicEffector::setPropName_inertialVelocity(std::string value) {
+   if (!value.empty()) {
+       this->propName_inertialVelocity = value;
+   } else {
+       bskLogger.bskLog(BSK_ERROR, "FacetDragDynamicEffector: propName_inertialVelocity variable must be a non-empty string");
+   }
+}
 
-    return;
+/*! This method is used to set the inertialAttitude property when facet drag is attached to a state effector rather than the hub
+ */
+void FacetDragDynamicEffector::setPropName_inertialAttitude(std::string value) {
+   if (!value.empty()) {
+       this->propName_inertialAttitude = value;
+   } else {
+       bskLogger.bskLog(BSK_ERROR, "FacetDragDynamicEffector: propName_inertialAttitude variable must be a non-empty string");
+   }
+}
+
+/*! This method updates the internal drag direction based on the spacecraft velocity vector.
+*/
+void FacetDragDynamicEffector::updateDragDir(){
+   Eigen::MRPd sigmaBN;
+   Eigen::Vector3d velocity;
+   // Determine which parent to use based on what's been linked
+   if (this->inertialAttitudeProperty != nullptr && this->inertialVelocityProperty != nullptr) {
+       // Attached to state effector: use properties
+       sigmaBN = (Eigen::Vector3d)(*this->inertialAttitudeProperty);
+       velocity = (*this->inertialVelocityProperty);
+   } else {
+       // Attached to hub: use states
+       sigmaBN = (Eigen::Vector3d)this->hubSigma->getState();
+       velocity = this->hubVelocity->getState();
+   }
+
+   Eigen::Matrix3d dcm_BN = sigmaBN.toRotationMatrix().transpose();
+
+   this->v_B = dcm_BN * velocity; // [m/s] sc velocity in body frame
+   this->v_hat_B = this->v_B / this->v_B.norm();
+
+   return;
 }
 
 /*! This method WILL implement a more complex flat-plate aerodynamics model with attitude

src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h

@@ -54,6 +54,7 @@ class FacetDragDynamicEffector: public SysModel, public DynamicEffector {
     FacetDragDynamicEffector();
     ~FacetDragDynamicEffector();
     void linkInStates(DynParamManager& states);
+    void linkInProperties(DynParamManager& properties); // To allow dynamic effector to attach to state effectors
     void computeForceTorque(double integTime, double timeStep);
     void Reset(uint64_t CurrentSimNanos);               //!< class method
     void UpdateState(uint64_t CurrentSimNanos);
@@ -74,10 +75,18 @@ class FacetDragDynamicEffector: public SysModel, public DynamicEffector {
     Eigen::Vector3d v_hat_B;                        //!< class variable
     BSKLogger bskLogger;                            //!< -- BSK Logging
 
+    /*! @brief setter for inertial velocity property name */
+    void setPropName_inertialVelocity(std::string value) override;
+    /*! @brief setter for inertial attitude property name */
+    void setPropName_inertialAttitude(std::string value) override;
+
 private:
     AtmoPropsMsgPayload atmoInData;
     SpacecraftGeometryData scGeometry;              //!< -- Struct to hold spacecraft facet data
 
+    // Property pointers (for state effector attachment)
+    Eigen::MatrixXd *inertialVelocityProperty;      //!< [m/s] velocity relative to inertial frame
+    Eigen::MatrixXd *inertialAttitudeProperty;      //!< attitude relative to inertial frame
 };
 
-#endif 
+#endif