Enable facet drag dynamic effector to link 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)
@@ -533,6 +564,34 @@ def setup_thrusterDynamicEffector():
 
     return(thruster, thFactory)
 
+def setup_facetDragDynamicEffector(stateEffProps):
+    facetDrag = facetDragDynamicEffector.FacetDragDynamicEffector()
+    facetDrag.ModelTag = "facetDragDynamicEffector"
+
+    facetDrag.setPropName_inertialPosition(stateEffProps.nameOfInertialPositionProperty)
+    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"
@@ -800,6 +859,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)
 
@@ -867,4 +929,4 @@ class stateEffectorProperties:
     r_PcP_P = [[0.0], [0.0], [0.0]] # individual COM for linkage that dynEff will be attached to
 
 if __name__ == "__main__":
-    effectorBranchingIntegratedTest(True, "hingedRigidBodies", True, "extForceTorque", True)
+    effectorBranchingIntegratedTest(True, "hingedRigidBodies", True, "facetDragDynamicEffector", True)

src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.cpp

@@ -31,7 +31,24 @@ FacetDragDynamicEffector::FacetDragDynamicEffector()
     this->atmoInData = {};  // Initialize atmosphere data to zero
     this->windInData = {};  // Initialize wind data to zero
 	this->numFacets = 0;
-	return;
+
+   // Initialize state pointers
+   this->hubSigma = nullptr;
+   this->hubVelocity = nullptr;
+
+   // Initialize property pointers
+   this->inertialPositionProperty = nullptr;
+   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.*/
@@ -106,13 +123,76 @@ void FacetDragDynamicEffector::linkInStates(DynParamManager& states){
 	this->hubVelocity = states.getStateObject(this->stateNameOfVelocity);
 }
 
+/*! 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::linkInProperties(DynParamManager& properties)
+{
+
+   // updateDragDir only requires these two to do calculations (Keep linking position in case you want to add altitude-dependent drag coefficients or other position-based effects later)
+   this->inertialAttitudeProperty = properties.getPropertyReference(this->propName_inertialAttitude);
+   this->inertialVelocityProperty = properties.getPropertyReference(this->propName_inertialVelocity);
+}
+
+/*! This method is used to set the inertialPosition property when facet drag is attached to a state effector rather than the hub
+ */
+void FacetDragDynamicEffector::setPropName_inertialPosition(std::string value) {
+   if (!value.empty()) {
+       this->propName_inertialPosition = value;
+   } else {
+       bskLogger.bskLog(BSK_ERROR, "FacetDragDynamicEffector: propName_inertialPosition variable must be a non-empty string");
+   }
+}
+
+/*! 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");
+   }
+}
+
+/*! 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 is used to set the inertialAngVelocity property when facet drag is attached to a state effector rather than the hub
+ */
+void FacetDragDynamicEffector::setPropName_inertialAngVelocity(std::string value) {
+   if (!value.empty()) {
+       this->propName_inertialAngVelocity = value;
+   } else {
+       bskLogger.bskLog(BSK_ERROR, "FacetDragDynamicEffector: propName_inertialAngVelocity variable must be a non-empty string");
+   }
+}
+
 /*! This method updates the internal drag direction based on the spacecraft velocity vector.
  * It accounts for wind velocity if the wind message is linked.
 */
 void FacetDragDynamicEffector::updateDragDir(){
-    Eigen::MRPd sigmaBN;
-    sigmaBN = (Eigen::Vector3d)this->hubSigma->getState();
-    Eigen::Matrix3d dcm_BN = sigmaBN.toRotationMatrix().transpose();
+   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();
 
     Eigen::Vector3d v_BN_N = this->hubVelocity->getState();
 
@@ -131,7 +211,7 @@ void FacetDragDynamicEffector::updateDragDir(){
         this->v_hat_B.setZero();
     }
 
-    return;
+   return;
 }
 
 /*! This method WILL implement a more complex flat-plate aerodynamics model with attitude

src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h

@@ -55,6 +55,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);
@@ -77,11 +78,36 @@ class FacetDragDynamicEffector: public SysModel, public DynamicEffector {
     Eigen::Vector3d v_hat_B;                        //!< -- Drag force direction in the body frame
     BSKLogger bskLogger;                            //!< -- BSK Logging
 
+    // State names (for hub attachment)
+    std::string stateNameOfSigma;
+    std::string stateNameOfVelocity;
+
+    // Property names (for state effector attachment)
+    std::string propName_inertialPosition;
+    std::string propName_inertialVelocity;
+    std::string propName_inertialAttitude;
+    std::string propName_inertialAngVelocity;
+
+    void setPropName_inertialPosition(std::string value) override;
+    void setPropName_inertialVelocity(std::string value) override;
+    void setPropName_inertialAttitude(std::string value) override;
+    void setPropName_inertialAngVelocity(std::string value) override;
+
+    std::string getPropName_inertialPosition() const { return this->propName_inertialPosition; }
+    std::string getPropName_inertialVelocity() const { return this->propName_inertialVelocity; }
+    std::string getPropName_inertialAttitude() const { return this->propName_inertialAttitude; }
+    std::string getPropName_inertialAngVelocity() const { return this->propName_inertialAngVelocity; }
+
 private:
     AtmoPropsMsgPayload atmoInData;
     WindMsgPayload windInData;
     SpacecraftGeometryData scGeometry;              //!< -- Struct to hold spacecraft facet data
 
+    // Property pointers (for state effector attachment)
+    Eigen::MatrixXd *inertialPositionProperty;      //!< [m] position relative to inertial frame
+    Eigen::MatrixXd *inertialVelocityProperty;      //!< [m/s] velocity relative to inertial frame
+    Eigen::MatrixXd *inertialAttitudeProperty;      //!< attitude relative to inertial frame
+    Eigen::MatrixXd *inertialAngVelocity;      //!< attitude relative to inertial frame
 };
 
 #endif