From 05aee4b1b859380bf7b441d00031d05bd93c1e7e Mon Sep 17 00:00:00 2001 From: Leah Kiner Date: Wed, 13 Dec 2023 17:02:45 -0700 Subject: [PATCH] Add optional facet articulation to the module --- .../test_unitFacetSRPDynamicEffector.py | 58 +++++++- .../facetSRPDynamicEffector.cpp | 129 +++++++++++++----- .../facetSRPDynamicEffector.h | 8 ++ 3 files changed, 152 insertions(+), 43 deletions(-) diff --git a/src/simulation/dynamics/facetSRPDynamicEffector/_UnitTest/test_unitFacetSRPDynamicEffector.py b/src/simulation/dynamics/facetSRPDynamicEffector/_UnitTest/test_unitFacetSRPDynamicEffector.py index fce2bbb4fb..5731c5e799 100644 --- a/src/simulation/dynamics/facetSRPDynamicEffector/_UnitTest/test_unitFacetSRPDynamicEffector.py +++ b/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 ) diff --git a/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.cpp b/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.cpp index bf9fe74123..5cb6dda91d 100644 --- a/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.cpp +++ b/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 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 *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>::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; } diff --git a/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h b/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h index f7cd2459e2..da40ca4e36 100644 --- a/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h +++ 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 facetDiffCoeffs; //!< Vector of facet diffuse reflection optical coefficients std::vector facetNormals_B; //!< Vector of facet normals expressed in B frame components std::vector facetLocationsPntB_B; //!< [m] Vector of facet COP locations wrt point B expressed in B frame components + std::vector 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 *tmpMsg); + void ReadMessages(); ReadFunctor sunInMsg; //!< Sun spice ephemeris input message + std::vector> 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 articulatedFacetDataBuffer; //!< Articulated facet angle data buffer + std::vector thetaList; //!< [rad] Vector of facet rotation angles }; #endif