Add optional facet articulation to the module

src/simulation/dynamics/facetSRPDynamicEffector/_UnitTest/test_unitFacetSRPDynamicEffector.py

@@ -20,6 +20,7 @@
 #   Module Name:        facetSRPDynamicEffector
 #   Author:             Leah Kiner
 #   Creation Date:      Dec 18 2022
+#   Last Updated:       Dec 13 2023
 #
 
 import os
@@ -42,7 +43,10 @@
 from Basilisk.simulation import spacecraft
 from Basilisk.architecture import messaging
 
-def test_facetSRPTestFunction(show_plots):
+# Vary the articulated facet initial angles
+@pytest.mark.parametrize("facetRotAngle1", [macros.D2R * -10.4, macros.D2R * 45.2, macros.D2R * 90.0, macros.D2R * 180.0])
+@pytest.mark.parametrize("facetRotAngle2", [macros.D2R * -28.0, macros.D2R * 45.2, macros.D2R * -90.0, macros.D2R * 180.0])
+def test_facetSRPTestFunction(show_plots, facetRotAngle1, facetRotAngle2):
     r"""
     **Validation Test Description**
 
@@ -57,11 +61,11 @@ def test_facetSRPTestFunction(show_plots):
         show_plots (bool): (True) Show plots, (False) Do not show plots
     """
 
-    [testResults, testMessage] = facetSRPTestFunction(show_plots)
+    [testResults, testMessage] = facetSRPTestFunction(show_plots, facetRotAngle1, facetRotAngle2)
 
     assert testResults < 1, testMessage
 
-def facetSRPTestFunction(show_plots):
+def facetSRPTestFunction(show_plots, facetRotAngle1, facetRotAngle2):
     testFailCount = 0                                           # Zero the unit test result counter
     testMessages = []                                           # Create an empty array to store the test log messages
 
@@ -103,11 +107,29 @@ def facetSRPTestFunction(show_plots):
     sunMsg = messaging.SpicePlanetStateMsg().write(sunStateMsg)
     gravFactory.gravBodies['sun'].planetBodyInMsg.subscribeTo(sunMsg)
 
+    # Create the articulated facet angle messages
+    facetRotAngle1MessageData = messaging.HingedRigidBodyMsgPayload()
+    facetRotAngle1MessageData.theta = facetRotAngle1
+    facetRotAngle1MessageData.thetaDot = 0.0
+    facetRotAngle1Message = messaging.HingedRigidBodyMsg().write(facetRotAngle1MessageData)
+
+    facetRotAngle2MessageData = messaging.HingedRigidBodyMsgPayload()
+    facetRotAngle2MessageData.theta = facetRotAngle2
+    facetRotAngle2MessageData.thetaDot = 0.0
+    facetRotAngle2Message = messaging.HingedRigidBodyMsg().write(facetRotAngle2MessageData)
+
     # Create an instance of the facetSRPDynamicEffector module to be tested
     srpEffector = facetSRPDynamicEffector.FacetSRPDynamicEffector()
     srpEffector.ModelTag = "srpEffector"
     numFacets = 10  # Total number of spacecraft facets
+    numArticulatedFacets = 4  # Number of articulated facets
+    srpEffector.numFacets = numFacets
+    srpEffector.numArticulatedFacets = numArticulatedFacets
     srpEffector.sunInMsg.subscribeTo(sunMsg)
+    srpEffector.addArticulatedFacet(facetRotAngle1Message)
+    srpEffector.addArticulatedFacet(facetRotAngle1Message)
+    srpEffector.addArticulatedFacet(facetRotAngle2Message)
+    srpEffector.addArticulatedFacet(facetRotAngle2Message)
     scObject.addDynamicEffector(srpEffector)
     unitTestSim.AddModelToTask(unitTaskName, srpEffector)
 
@@ -142,13 +164,25 @@ def facetSRPTestFunction(show_plots):
                        np.array([-4.5, 0.0, 0.75]),
                        np.array([-4.5, 0.0, 0.75])]
 
+        # Define facet articulation axes in B frame components
+        articulationAxes_B = [np.array([0.0, 0.0, 0.0]),
+                              np.array([0.0, 0.0, 0.0]),
+                              np.array([0.0, 0.0, 0.0]),
+                              np.array([0.0, 0.0, 0.0]),
+                              np.array([0.0, 0.0, 0.0]),
+                              np.array([0.0, 0.0, 0.0]),
+                              np.array([1.0, 0.0, 0.0]),
+                              np.array([1.0, 0.0, 0.0]),
+                              np.array([-1.0, 0.0, 0.0]),
+                              np.array([-1.0, 0.0, 0.0])]
+
         # Define facet optical coefficients
         specCoeff = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
         diffCoeff = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
 
         # Populate the srpEffector spacecraft geometry structure with the facet information
         for i in range(numFacets):
-            srpEffector.addFacet(facetAreas[i], specCoeff[i], diffCoeff[i], facetNormals_B[i], locationsPntB_B[i])
+            srpEffector.addFacet(facetAreas[i], specCoeff[i], diffCoeff[i], facetNormals_B[i], locationsPntB_B[i], articulationAxes_B[i])
     except:
         testFailCount += 1
         testMessages.append("ERROR: FacetSRP unit test failed while setting facet parameters.")
@@ -214,7 +248,7 @@ def facetSRPTestFunction(show_plots):
     accuracy = 1e-3
     test_val = np.zeros([3,])
     for i in range(len(facetAreas)):
-        test_val += checkFacetSRPForce(facetAreas[i], specCoeff[i], diffCoeff[i], facetNormals_B[i], articulationAxes_B[i], sigma_BN[-1], r_BN_N[-1], r_SN_N[-1])
+        test_val += checkFacetSRPForce(i, facetRotAngle1, facetRotAngle2, facetAreas[i], specCoeff[i], diffCoeff[i], facetNormals_B[i], articulationAxes_B[i], sigma_BN[-1], r_BN_N[-1], r_SN_N[-1])
 
     if not unitTestSupport.isArrayEqual(srpForce_B[-1, :], test_val, 3, accuracy):
         testFailCount += 1
@@ -229,7 +263,7 @@ def facetSRPTestFunction(show_plots):
 
     return testFailCount, testMessages
 
-def checkFacetSRPForce(area, specCoeff, diffCoeff, facetNormal, sigma_BN, scPos, sunPos):
+def checkFacetSRPForce(index, facetRotAngle1, facetRotAngle2, area, specCoeff, diffCoeff, facetNormal, facetArticAxis, sigma_BN, scPos, sunPos):
     # Define required constants
     speedLight = 299792458.0  # [m/s] Speed of light
     AstU = 149597870700.0  # [m] Astronomical unit
@@ -246,6 +280,16 @@ def checkFacetSRPForce(area, specCoeff, diffCoeff, facetNormal, sigma_BN, scPos,
     # Determine unit direction vector pointing from sc to the Sun
     sHat = r_SB_B / np.linalg.norm(r_SB_B)
 
+    # Rotate the articulated facet normal vectors
+    if (index == 6 or index == 7):
+        prv_FF0 = facetRotAngle1 * facetArticAxis
+        dcm_FF0 = rbk.PRV2C(prv_FF0)
+        facetNormal = np.matmul(dcm_FF0, facetNormal)
+    if (index == 8 or index == 9):
+        prv_FF0 = facetRotAngle2 * facetArticAxis
+        dcm_FF0 = rbk.PRV2C(prv_FF0)
+        facetNormal = np.matmul(dcm_FF0, facetNormal)
+
     # Determine the facet projected area
     cosTheta = np.dot(sHat, facetNormal)
     projArea = area * cosTheta
@@ -265,4 +309,6 @@ def checkFacetSRPForce(area, specCoeff, diffCoeff, facetNormal, sigma_BN, scPos,
 if __name__=="__main__":
     facetSRPTestFunction(
         False,  # show plots
+        macros.D2R * -10.0,  # facetRotAngle1
+        macros.D2R * 45.0,  # facetRotAngle2
     )

src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.cpp

@@ -18,6 +18,9 @@
  */
 
 #include "facetSRPDynamicEffector.h"
+#include "architecture/utilities/rigidBodyKinematics.h"
+#include "architecture/utilities/avsEigenSupport.h"
+#include "architecture/utilities/linearAlgebra.h"
 #include <cmath>
 
 const double speedLight = 299792458.0;  // [m/s] Speed of light
@@ -29,6 +32,7 @@ FacetSRPDynamicEffector::FacetSRPDynamicEffector() {
     this->forceExternal_B.fill(0.0);
     this->torqueExternalPntB_B.fill(0.0);
     this->numFacets = 0;
+    this->numArticulatedFacets = 0;
 }
 
 /*! The destructor */
@@ -49,19 +53,29 @@ void FacetSRPDynamicEffector::Reset(uint64_t callTime) {
  @param diffCoeff  Facet diffuse reflection optical coefficient
  @param normal_B  Facet normal expressed in B frame components
  @param locationPntB_B  [m] Facet location wrt point B expressed in B frame components
+ @param facetRotAxes_B  Facet articulation axis expressed in B frame components
 */
 void FacetSRPDynamicEffector::addFacet(double area,
                                        double specCoeff,
                                        double diffCoeff,
                                        Eigen::Vector3d normal_B,
-                                       Eigen::Vector3d locationPntB_B)
-{
+                                       Eigen::Vector3d locationPntB_B,
+                                       Eigen::Vector3d rotAxis_B) {
     this->scGeometry.facetAreas.push_back(area);
     this->scGeometry.facetSpecCoeffs.push_back(specCoeff);
     this->scGeometry.facetDiffCoeffs.push_back(diffCoeff);
     this->scGeometry.facetNormals_B.push_back(normal_B);
     this->scGeometry.facetLocationsPntB_B.push_back(locationPntB_B);
-    this->numFacets = this->numFacets + 1;
+    this->scGeometry.facetRotAxes_B.push_back(rotAxis_B);
+}
+
+/*! This method subscribes the articulated facet angle input messages to the module
+articulatedFacetDataInMsgs input message
+ @return void
+ @param tmpMsg  hingedRigidBody input message containing facet articulation angle data
+*/
+void FacetSRPDynamicEffector::addArticulatedFacet(Message<HingedRigidBodyMsgPayload> *tmpMsg) {
+    this->articulatedFacetDataInMsgs.push_back(tmpMsg->addSubscriber());
 }
 
 /*! This method is used to link the faceted SRP effector to the hub attitude and position,
@@ -74,79 +88,120 @@ void FacetSRPDynamicEffector::linkInStates(DynParamManager& states) {
     this->hubPosition = states.getStateObject("hubPosition");
 }
 
-/*! This method computes the body forces and torques for the SRP effector.
+/*! This method reads the Sun state input message. If time-varying facet articulations are considered,
+the articulation angle messages are also read
  @return void
- @param integTime  [s] Time the method is called
- @param timeStep  [s] Simulation time step
 */
-void FacetSRPDynamicEffector::computeForceTorque(double integTime, double timeStep)
+void FacetSRPDynamicEffector::ReadMessages()
 {
-    // Read the input message
-    SpicePlanetStateMsgPayload sunMsgBuffer;
-    sunMsgBuffer = sunInMsg.zeroMsgPayload;
-    sunMsgBuffer = this->sunInMsg();
+    // Read the Sun state input message
+    if (this->sunInMsg.isLinked() && this->sunInMsg.isWritten()) {
+        SpicePlanetStateMsgPayload sunMsgBuffer;
+        sunMsgBuffer = sunInMsg.zeroMsgPayload;
+        sunMsgBuffer = this->sunInMsg();
+        this->r_SN_N = cArray2EigenVector3d(sunMsgBuffer.PositionVector);
+    }
 
-    // Calculate the Sun position with respect to the inertial frame, expressed in inertial frame components
-    this->r_SN_N = cArray2EigenVector3d(sunMsgBuffer.PositionVector);
+    // Read the facet articulation angle data
+    if (this->articulatedFacetDataInMsgs.size() == this->numArticulatedFacets) {
+        std::vector<ReadFunctor<HingedRigidBodyMsgPayload>>::iterator it;
+        uint64_t index;
+        HingedRigidBodyMsgPayload facetAngleMsg;
+        for (it = this->articulatedFacetDataInMsgs.begin(), index = 0; it != this->articulatedFacetDataInMsgs.end(); it++, index++) {
+            if (it->isLinked() && it->isWritten()) {
+                    facetAngleMsg = this->articulatedFacetDataInMsgs.at(index)();
+                    this->thetaList.push_back(facetAngleMsg.theta);
+                }
+            else if (!it->isLinked()) {
+                    bskLogger.bskLog(BSK_ERROR, "FACET HINGED RIGID BODY ARTICULATION MSG NOT LINKED.");
+                }
+        }
+    } else {
+        bskLogger.bskLog(BSK_ERROR, "NUMBER OF ARTICULATED FACETS DOES NOT MATCH COUNTED VALUE.");
+    }
+}
+
+/*! This method computes the srp force and torque acting about the hub point B in B frame components
+ @return void
+ @param callTime  [s] Time the method is called
+ @param timeStep  [s] Simulation time step
+*/
+void FacetSRPDynamicEffector::computeForceTorque(double callTime, double timeStep) {
+    // Read the articulated facet information
+    ReadMessages();
 
-    // Compute dcm_BN using MRP transformation
-    Eigen::MRPd sigmaBN;
-    sigmaBN = (Eigen::Vector3d)this->hubSigma->getState();
-    Eigen::Matrix3d dcm_BN = sigmaBN.toRotationMatrix().transpose();
+    // Compute dcm_BN
+    Eigen::MRPd sigma_BN;
+    sigma_BN = (Eigen::Vector3d)this->hubSigma->getState();
+    Eigen::Matrix3d dcm_BN = sigma_BN.toRotationMatrix().transpose();
 
-    // Store the hub B frame position with respect to the inertial frame
+    // Grab the current spacecraft inertial position
     Eigen::Vector3d r_BN_N = this->hubPosition->getState();
 
-    // Calculate the vector pointing from point B on the spacecraft to the Sun
+    // Calculate the Sun unit direction vector relative to point B in B frame components
     Eigen::Vector3d r_SB_B = dcm_BN * (this->r_SN_N - r_BN_N);
-
-    // Calculate the unit vector pointing from point B on the spacecraft to the Sun
     Eigen::Vector3d sHat = r_SB_B / r_SB_B.norm();
 
-    // Define local vectors for the facet force and torque storage
+    // Define local srp force and torque storage vectors
     Eigen::Vector3d facetSRPForcePntB_B;
     Eigen::Vector3d facetSRPTorquePntB_B;
     Eigen::Vector3d totalSRPForcePntB_B;
     Eigen::Vector3d totalSRPTorquePntB_B;
 
     // Zero storage information
-    double projectedArea = 0.0;
-    double cosTheta = 0.0;
+    this->forceExternal_B.setZero();
+    this->torqueExternalPntB_B.setZero();
     facetSRPForcePntB_B.setZero();
     facetSRPTorquePntB_B.setZero();
     totalSRPForcePntB_B.setZero();
     totalSRPTorquePntB_B.setZero();
-    this->forceExternal_B.setZero();
-    this->torqueExternalPntB_B.setZero();
-    
-    // Calculate the SRP pressure acting on point B
+    double projectedArea = 0.0;
+    double cosTheta = 0.0;
+
+    // Calculate the SRP pressure acting at the current spacecraft location
     double numAU = AstU / r_SB_B.norm();
     double SRPPressure = (solarRadFlux / speedLight) * numAU * numAU;
 
-    // Loop through the facets and calculate the SRP force and torque acting on point B
-    for(int i = 0; i < this->numFacets; i++)
-    {
+    // Loop through the facets and calculate the SRP force and torque acting on the spacecraft about point B
+    for(int i = 0; i < this->numFacets; i++) {
+        // Determine the current facet normal vector if the facet articulates
+        if ((this->numArticulatedFacets != 0) && (i >= (this->numFacets - this->numArticulatedFacets))) {
+            uint64_t articulatedIndex = this->numArticulatedFacets - (this->numFacets - i);
+            double articulationAngle = thetaList.at(articulatedIndex);
+
+            // Determine the required DCM that rotates the facet normal vector through the articulation angle
+            double prv_FF0[3] = {articulationAngle * scGeometry.facetRotAxes_B[i][0],
+                               articulationAngle * scGeometry.facetRotAxes_B[i][1],
+                               articulationAngle * scGeometry.facetRotAxes_B[i][2]};
+            double dcmFF0[3][3];
+            PRV2C(prv_FF0, dcmFF0);
+            Eigen::Matrix3d dcm_FF0;
+            dcm_FF0 = c2DArray2EigenMatrix3d(dcmFF0);
+
+            // Rotate the facet normal vector through the current articulation angle
+            this->scGeometry.facetNormals_B[i] = dcm_FF0 * this->scGeometry.facetNormals_B[i];
+        }
+
+        // Determine the facet projected area
         cosTheta = this->scGeometry.facetNormals_B[i].dot(sHat);
         projectedArea = this->scGeometry.facetAreas[i] * cosTheta;
 
-        if(projectedArea > 0.0){
-            // Compute the SRP force acting on the ith facet
+        // Compute the SRP force and torque acting on the facet only if the facet is in view of the Sun
+        if(projectedArea > 0.0) {
             facetSRPForcePntB_B = -SRPPressure * projectedArea
                                   * ( (1-this->scGeometry.facetSpecCoeffs[i])
                                   * sHat + 2 * ( (this->scGeometry.facetDiffCoeffs[i] / 3)
                                   + this->scGeometry.facetSpecCoeffs[i] * cosTheta)
                                   * this->scGeometry.facetNormals_B[i] );
-
-            // Compute the SRP torque acting on the ith facet
             facetSRPTorquePntB_B = this->scGeometry.facetLocationsPntB_B[i].cross(facetSRPForcePntB_B);
 
-            // Compute the total SRP force and torque acting on the spacecraft
+            // Add the facet contribution to the total SRP force and torque acting on the spacecraft
             totalSRPForcePntB_B = totalSRPForcePntB_B + facetSRPForcePntB_B;
             totalSRPTorquePntB_B = totalSRPTorquePntB_B + facetSRPTorquePntB_B;
         }
     }
 
-    // Write the total SRP force and torque local variables to the dynamic effector variables
+    // Update the force and torque vectors in the dynamic effector base class
     this->forceExternal_B = totalSRPForcePntB_B;
     this->torqueExternalPntB_B = totalSRPTorquePntB_B;
 }

src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h

@@ -27,6 +27,7 @@
 #include "architecture/utilities/bskLogging.h"
 #include "architecture/messaging/messaging.h"
 #include "architecture/msgPayloadDefC/SpicePlanetStateMsgPayload.h"
+#include "architecture/msgPayloadDefC/HingedRigidBodyMsgPayload.h"
 #include "simulation/dynamics/_GeneralModuleFiles/stateData.h"
 #include "architecture/utilities/avsEigenSupport.h"
 #include "architecture/utilities/avsEigenMRP.h"
@@ -38,6 +39,7 @@ typedef struct {
     std::vector<double> facetDiffCoeffs;                              //!< Vector of facet diffuse reflection optical coefficients
     std::vector<Eigen::Vector3d> facetNormals_B;                      //!< Vector of facet normals expressed in B frame components
     std::vector<Eigen::Vector3d> facetLocationsPntB_B;                //!< [m] Vector of facet COP locations wrt point B expressed in B frame components
+    std::vector<Eigen::Vector3d> facetRotAxes_B;                      //!< [m] Vector of facet rotation axes expressed in B frame components
 }FacetedSRPSpacecraftGeometryData;
 
 /*! @brief Faceted Solar Radiation Pressure Dynamic Effector */
@@ -54,15 +56,21 @@ class FacetSRPDynamicEffector: public SysModel, public DynamicEffector {
                   Eigen::Vector3d normal_B,
                   Eigen::Vector3d locationPntB_B,
                   Eigen::Vector3d rotAxis_B);                                            //!< Method for adding facets to the spacecraft geometry structure
+    void addArticulatedFacet(Message<HingedRigidBodyMsgPayload> *tmpMsg);
+    void ReadMessages();
 
     ReadFunctor<SpicePlanetStateMsgPayload> sunInMsg;                                    //!< Sun spice ephemeris input message
+    std::vector<ReadFunctor<HingedRigidBodyMsgPayload>> articulatedFacetDataInMsgs;      //!< Articulated facet angle data input message
     uint64_t numFacets;                                                                  //!< Total number of spacecraft facets
+    uint64_t numArticulatedFacets;                                                       //!< Number of articulated facets
     Eigen::Vector3d r_SN_N;                                                              //!< [m] Sun inertial position vector
     StateData *hubPosition;                                                              //!< [m] Hub inertial position vector
     StateData *hubSigma;                                                                 //!< Hub MRP inertial attitude
 
 private:
     FacetedSRPSpacecraftGeometryData scGeometry;                                         //!< Spacecraft facet data structure
+    std::vector<HingedRigidBodyMsgPayload> articulatedFacetDataBuffer;                   //!< Articulated facet angle data buffer
+    std::vector<double> thetaList;                                                       //!< [rad] Vector of facet rotation angles
 };
 
 #endif