diff --git a/docs/source/Learn/bskPrinciples/bskPrinciples-11.rst b/docs/source/Learn/bskPrinciples/bskPrinciples-11.rst index f703dc7f48..2805eb3355 100644 --- a/docs/source/Learn/bskPrinciples/bskPrinciples-11.rst +++ b/docs/source/Learn/bskPrinciples/bskPrinciples-11.rst @@ -206,8 +206,8 @@ special case of prescribed effector branching explained in :ref:`prescribedMotio Hinged Rigid Bodies - - + + diff --git a/docs/source/Support/bskReleaseNotes.rst b/docs/source/Support/bskReleaseNotes.rst index 7d0b169a1c..d33142207f 100644 --- a/docs/source/Support/bskReleaseNotes.rst +++ b/docs/source/Support/bskReleaseNotes.rst @@ -87,6 +87,7 @@ Version |release| :ref:`scenarioPrescribedMotionWithTranslationBranching` and :ref:`scenarioPrescribedMotionWithRotationBranching`. - Fixed a bug where :ref:`spinningBodyOneDOFStateEffector` and :ref:`spinningBodyNDOFStateEffector` both registered their states under the same name, resulting in overwriting and a ``BSK_ERROR``. +- Added support for :ref:`hingedRigidBodyStateEffector` to be the parent for Dynamic Effectors. Version 2.8.0 (August 30, 2025) diff --git a/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.cpp b/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.cpp index 1704547a47..1a470da66c 100755 --- a/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.cpp +++ b/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.cpp @@ -44,8 +44,13 @@ HingedRigidBodyStateEffector::HingedRigidBodyStateEffector() this->IPntS_S.Identity(); this->r_HB_B.setZero(); this->dcm_HB.Identity(); + this->nameOfThetaState = "hingedRigidBodyTheta" + std::to_string(this->effectorID); this->nameOfThetaDotState = "hingedRigidBodyThetaDot" + std::to_string(this->effectorID); + this->nameOfInertialPositionProperty = "hingedRigidBodyInertialPosition" + std::to_string(this->effectorID); + this->nameOfInertialVelocityProperty = "hingedRigidBodyInertialVelocity" + std::to_string(this->effectorID); + this->nameOfInertialAttitudeProperty = "hingedRigidBodyInertialAttitude" + std::to_string(this->effectorID); + this->nameOfInertialAngVelocityProperty = "hingedRigidBodyInertialAngVelocity" + std::to_string(this->effectorID); this->effectorID++; return; @@ -81,10 +86,10 @@ void HingedRigidBodyStateEffector::writeOutputStateMessages(uint64_t CurrentCloc SCStatesMsgPayload configLogMsg; configLogMsg = this->hingedRigidBodyConfigLogOutMsg.zeroMsgPayload; // Note, logging the hinge frame S is the body frame B of that object - eigenVector3d2CArray(this->r_SN_N, configLogMsg.r_BN_N); - eigenVector3d2CArray(this->v_SN_N, configLogMsg.v_BN_N); - eigenVector3d2CArray(this->sigma_SN, configLogMsg.sigma_BN); - eigenVector3d2CArray(this->omega_SN_S, configLogMsg.omega_BN_B); + eigenMatrixXd2CArray(*this->r_SN_N, configLogMsg.r_BN_N); + eigenMatrixXd2CArray(*this->v_SN_N, configLogMsg.v_BN_N); + eigenMatrixXd2CArray(*this->sigma_SN, configLogMsg.sigma_BN); + eigenMatrixXd2CArray(*this->omega_SN_S, configLogMsg.omega_BN_B); this->hingedRigidBodyConfigLogOutMsg.write(&configLogMsg, this->moduleID, CurrentClock); } @@ -114,6 +119,20 @@ void HingedRigidBodyStateEffector::linkInStates(DynParamManager& statesIn) return; } +/*! This method attaches a dynamicEffector to the specified HRB + @param newDynamicEffector the dynamic effector to be attached to the HRB + @param segment the integer number segment to be attached to (default segment=1) */ +void HingedRigidBodyStateEffector::addDynamicEffector(DynamicEffector *newDynamicEffector, int segment) +{ + if (segment != 1) { + bskLogger.bskLog(BSK_ERROR, "Specifying attachment to a non-existent hinged rigid body linkage because this is 1DOF."); + } + + this->assignStateParamNames(newDynamicEffector); + + this->dynEffectors.push_back(newDynamicEffector); +} + /*! This method allows the HRB state effector to register its states: theta and thetaDot with the dyn param manager */ void HingedRigidBodyStateEffector::registerStates(DynParamManager& states) { @@ -127,9 +146,28 @@ void HingedRigidBodyStateEffector::registerStates(DynParamManager& states) thetaDotInitMatrix(0,0) = this->thetaDotInit; this->thetaDotState->setState(thetaDotInitMatrix); + registerProperties(states); + return; } +/*! This method registers the HRB inertial properties with the dynamic parameter manager and links + them into dependent dynamic effectors */ +void HingedRigidBodyStateEffector::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 HRB state effector to provide its contributions to the mass props and mass prop rates of the spacecraft */ void HingedRigidBodyStateEffector::updateEffectorMassProps(double integTime) @@ -196,6 +234,18 @@ void HingedRigidBodyStateEffector::updateContributions(double integTime, BackSub gLocal_N = g_N; g_P = dcm_PN*gLocal_N; + // 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; // here parent frame is S, not B because dynamic effector attached to state effector not hub + attBodyTorquePntS_S+= (*dynIt)->torqueExternalPntB_B; + } + // - Define omega_BN_S this->omega_BN_B = omega_BN_B; this->omegaLoc_PN_P = this->omega_BN_B; @@ -215,7 +265,7 @@ void HingedRigidBodyStateEffector::updateContributions(double integTime, BackSub Eigen::Vector3d gravityTorquePntH_P; gravityTorquePntH_P = -this->d*this->sHat1_P.cross(this->mass*g_P); this->cTheta = 1.0/(this->IPntS_S(1,1) + this->mass*this->d*this->d)*(this->u -this->k*(this->theta-this->thetaRef) - this->c*(this->thetaDot-this->thetaDotRef) - + this->sHat2_P.dot(gravityTorquePntH_P) + (this->IPntS_S(2,2) - this->IPntS_S(0,0) + + this->sHat2_P.dot(gravityTorquePntH_P + this->dcm_SP.transpose() * attBodyTorquePntS_S) + (this->IPntS_S(2,2) - this->IPntS_S(0,0) + this->mass*this->d*this->d)*this->omega_PN_S(2)*this->omega_PN_S(0) - this->mass*this->d* this->sHat3_P.transpose()*this->omegaTildeLoc_PN_P*this->omegaTildeLoc_PN_P*this->r_HP_P); @@ -224,7 +274,8 @@ void HingedRigidBodyStateEffector::updateContributions(double integTime, BackSub backSubContr.matrixA = this->mass*this->d*this->sHat3_P*this->aTheta.transpose(); backSubContr.matrixB = this->mass*this->d*this->sHat3_P*this->bTheta.transpose(); backSubContr.vecTrans = -(this->mass*this->d*this->thetaDot*this->thetaDot*this->sHat1_P - + this->mass*this->d*this->cTheta*this->sHat3_P); + + this->mass*this->d*this->cTheta*this->sHat3_P) + + this->dcm_SP.transpose() * attBodyForce_S; // - Define rotational matrice contributions backSubContr.matrixC = (this->IPntS_S(1,1)*this->sHat2_P + this->mass*this->d*this->rTilde_SP_P*this->sHat3_P) @@ -234,7 +285,8 @@ void HingedRigidBodyStateEffector::updateContributions(double integTime, BackSub Eigen::Matrix3d intermediateMatrix; backSubContr.vecRot = -((this->thetaDot*this->omegaTildeLoc_PN_P + this->cTheta*intermediateMatrix.Identity()) *(this->IPntS_S(1,1)*this->sHat2_P + this->mass*this->d*this->rTilde_SP_P*this->sHat3_P) - + this->mass*this->d*this->thetaDot*this->thetaDot*this->rTilde_SP_P*this->sHat1_P); + + this->mass*this->d*this->thetaDot*this->thetaDot*this->rTilde_SP_P*this->sHat1_P) + + (this->dcm_SP.transpose() * attBodyTorquePntS_S) + eigenTilde(this->r_SP_P) * (this->dcm_SP.transpose() * attBodyForce_S); return; } @@ -373,19 +425,19 @@ void HingedRigidBodyStateEffector::computePanelInertialStates() Eigen::Matrix3d dcm_NP = sigmaBN.toRotationMatrix(); // assumes P and B are idential Eigen::Matrix3d dcm_SN; dcm_SN = this->dcm_SP*dcm_NP.transpose(); - this->sigma_SN = eigenMRPd2Vector3d(eigenC2MRP(dcm_SN)); + *this->sigma_SN = eigenMRPd2Vector3d(eigenC2MRP(dcm_SN)); // inertial angular velocity Eigen::Vector3d omega_BN_B; omega_BN_B = this->omega_BN_B; - this->omega_SN_S = this->dcm_SP * ( omega_BN_B + this->thetaDot*this->sHat2_P); + *this->omega_SN_S = this->dcm_SP * ( omega_BN_B + this->thetaDot*this->sHat2_P); // inertial position vector - this->r_SN_N = (dcm_NP * this->r_SP_P) + (Eigen::Vector3d)(*this->inertialPositionProperty); + *this->r_SN_N = (dcm_NP * this->r_SP_P) + (Eigen::Vector3d)(*this->inertialPositionProperty); // inertial velocity vector - this->v_SN_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) + *this->v_SN_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) + this->d * this->thetaDot * this->sHat3_P - this->d * (omega_BN_B.cross(this->sHat1_P)) + omega_BN_B.cross(this->r_HP_P); diff --git a/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.h b/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.h index e2ca7fb9e8..1a135c1f09 100755 --- a/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.h +++ b/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.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" @@ -46,6 +47,10 @@ class HingedRigidBodyStateEffector : public StateEffector, public SysModel { double thetaDotRef; //!< [rad/s] hinged rigid body reference 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::Matrix3d IPntS_S; //!< [kg-m^2] Inertia of hinged rigid body about point S in S frame components Eigen::Vector3d r_HB_B; //!< [m] vector pointing from body frame origin to Hinge location Eigen::Matrix3d dcm_HB; //!< -- DCM from body frame to hinge frame @@ -55,59 +60,82 @@ class HingedRigidBodyStateEffector : public StateEffector, public SysModel { Message hingedRigidBodyConfigLogOutMsg; //!< panel state config log message name HingedRigidBodyMsgPayload HRBoutputStates; //!< instance of messaging system message struct BSKLogger bskLogger; //!< -- BSK Logging + std::vector dynEffectors; //!< Vector of dynamic effectors attached private: static uint64_t effectorID; //!< [] ID number of this panel - double theta; //!< [rad] hinged rigid body angle - double thetaDot; //!< [rad/s] hinged rigid body angle rate - double cTheta; //!< -- term needed for back substitution double u; //!< [N-m] optional motor torque - Eigen::Vector3d r_HP_P; //!< [m] vector pointing from primary body frame P origin to Hinge location. If a single spacecraft body is modeled than P is the same as B - Eigen::Matrix3d dcm_HP; //!< -- DCM from primary body frame to hinge frame + + 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; //!< -- term needed for back substitution Eigen::Vector3d bTheta; //!< -- term needed for back substitution - Eigen::Matrix3d rTilde_HP_P; //!< -- Tilde matrix of rHB_B - Eigen::Matrix3d dcm_SH; //!< -- DCM from hinge to hinged rigid body frame, S - Eigen::Matrix3d dcm_SP; //!< -- DCM from body to S frame - Eigen::Vector3d omega_PN_S; //!< [rad/s] omega_BN in S frame components + double cTheta; //!< -- term needed for back substitution + + // Vector quantities + Eigen::Vector3d r_HP_P; //!< [m] vector pointing from primary body frame P origin to Hinge location. If a single spacecraft body is modeled than P is the same as B + Eigen::Vector3d omega_PN_S; //!< [rad/s] omega_BN in S frame components Eigen::Vector3d sHat1_P; //!< -- unit direction vector for the first axis of the S frame Eigen::Vector3d sHat2_P; //!< -- unit direction vector for the second axis of the S frame Eigen::Vector3d sHat3_P; //!< -- unit direction vector for the third axis of the S frame Eigen::Vector3d r_SP_P; //!< -- Vector pointing from B to CoM of hinged rigid body in B frame components - Eigen::Matrix3d rTilde_SP_P; //!< -- Tilde matrix of rSB_B Eigen::Vector3d rPrime_SP_P; //!< [m/s] Body time derivative of rSB_B - Eigen::Matrix3d rPrimeTilde_SP_P; //!< -- Tilde matrix of rPrime_SB_B - Eigen::Matrix3d ISPrimePntS_P; //!< [kg-m^2/s] time body derivative IPntS in body frame components Eigen::Vector3d omegaLoc_PN_P; //!< [rad/s] local copy of omegaBN - Eigen::Matrix3d omegaTildeLoc_PN_P; //!< -- tilde matrix of omegaBN - Eigen::Vector3d r_SN_N; //!< [m] position vector of hinge CM S relative to inertial frame - Eigen::Vector3d v_SN_N; //!< [m/s] inertial velocity vector of S relative to inertial frame - Eigen::Vector3d sigma_SN; //!< -- MRP attitude of panel frame S relative to inertial frame - Eigen::Vector3d omega_SN_S; //!< [rad/s] inertial panel frame angular velocity vector - Eigen::MRPd sigma_BN{0.0, 0.0, 0.0}; //!< Hub/Inertial attitude represented by MRP Eigen::Vector3d omega_BN_B{0.0, 0.0, 0.0}; //!< Hub/Inertial angular velocity vector in B frame components - StateData *thetaState; //!< -- state manager of theta for hinged rigid body - Eigen::MatrixXd *inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase - Eigen::MatrixXd *inertialVelocityProperty; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase - StateData *thetaDotState; //!< -- state manager of thetaDot for hinged rigid body - Eigen::MatrixXd *c_B; //!< [m] Vector from point B to CoM of s/c in B frame components + // Matrix quantities + Eigen::Matrix3d dcm_HP; //!< -- DCM from primary body frame to hinge frame + Eigen::Matrix3d dcm_SH; //!< -- DCM from hinge to hinged rigid body frame, S + Eigen::Matrix3d dcm_SP; //!< -- DCM from body to S frame + Eigen::Matrix3d rTilde_HP_P; //!< -- Tilde matrix of rHB_B + Eigen::Matrix3d rTilde_SP_P; //!< -- Tilde matrix of rSB_B + Eigen::Matrix3d rPrimeTilde_SP_P; //!< -- Tilde matrix of rPrime_SB_B + Eigen::Matrix3d ISPrimePntS_P; //!< [kg-m^2/s] time body derivative IPntS in body frame components + Eigen::Matrix3d omegaTildeLoc_PN_P; //!< -- tilde matrix of omegaBN + Eigen::MRPd sigma_BN{0.0, 0.0, 0.0}; //!< Hub/Inertial attitude represented by MRP + + // Hinged rigid body properties + Eigen::MatrixXd* r_SN_N; //!< [m] position vector of hinge CM S relative to inertial frame N + Eigen::MatrixXd* v_SN_N; //!< [m/s] inertial velocity vector of S relative to inertial frame N + Eigen::MatrixXd* sigma_SN; //!< -- MRP attitude of panel frame S relative to inertial frame + Eigen::MatrixXd* omega_SN_S; //!< [rad/s] inertial panel frame angular velocity vector + + // Hub properties + Eigen::MatrixXd* inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase + Eigen::MatrixXd* inertialVelocityProperty; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase - Eigen::MatrixXd *cPrime_B; //!< [m/s] Body time derivative of vector c_B in B frame components + Eigen::MatrixXd* c_B; //!< [m] Vector from point B to CoM of s/c in B frame components + Eigen::MatrixXd* cPrime_B; //!< [m/s] Body time derivative of vector c_B in B frame components + + // States + double theta; //!< [rad] hinged rigid body angle + double thetaDot; //!< [rad/s] hinged rigid body angle rate + StateData* thetaState; //!< -- state manager of theta for hinged rigid body + StateData* thetaDotState; //!< -- state manager of thetaDot for hinged rigid body public: HingedRigidBodyStateEffector(); //!< -- Contructor ~HingedRigidBodyStateEffector(); //!< -- Destructor - void writeOutputStateMessages(uint64_t CurrentClock); - void UpdateState(uint64_t CurrentSimNanos); - void registerStates(DynParamManager& statesIn); //!< -- Method for registering the HRB states - void linkInStates(DynParamManager& states); //!< -- Method for getting access to other states - void updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N); //!< -- Method for back-sub contributions - void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN); //!< -- Method for HRB to compute its derivatives - void updateEffectorMassProps(double integTime); //!< -- 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); //!< -- Computing energy and momentum for HRBs - void calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B); //!< -- Force and torque on s/c due to HRBs - void prependSpacecraftNameToStates(); //!< class method + void writeOutputStateMessages(uint64_t CurrentClock) override; + void UpdateState(uint64_t CurrentSimNanos) override; + void registerStates(DynParamManager& statesIn) override; //!< -- Method for registering the HRB states + void linkInStates(DynParamManager& states) override; //!< -- Method for getting access to other states + void addDynamicEffector(DynamicEffector *newDynamicEffector, int segment = 1) override; //!< -- Method for adding attached dynamic effector + void registerProperties(DynParamManager& states) override; //!< -- Method for registering the HRB 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-sub contributions + void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) override; //!< -- Method for HRB 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) override; //!< -- Computing energy and momentum for HRBs + void calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B) override; //!< -- Force and torque on s/c due to HRBs + void prependSpacecraftNameToStates() override; //!< class method private: void computePanelInertialStates(); diff --git a/src/simulation/dynamics/_GeneralModuleFiles/_UnitTestDynamics/test_effectorBranching_integrated.py b/src/simulation/dynamics/_GeneralModuleFiles/_UnitTestDynamics/test_effectorBranching_integrated.py index 87901be3a5..8c0ef2e052 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/_UnitTestDynamics/test_effectorBranching_integrated.py +++ b/src/simulation/dynamics/_GeneralModuleFiles/_UnitTestDynamics/test_effectorBranching_integrated.py @@ -74,7 +74,7 @@ # 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), + ("hingedRigidBodies", True), # ("dualHingedRigidBodies", True), # ("nHingedRigidBodies", True), ("spinningBodiesOneDOF", True), @@ -250,6 +250,9 @@ def effectorBranchingIntegratedTest(show_plots, stateEffector, isParent, dynamic elif stateEffector == "spinningBodiesNDOF": stateEff, stateEffProps = setup_spinningBodiesNDOF() segment = 4 + elif stateEffector == "hingedRigidBodies": + stateEff, stateEffProps = setup_hingedRigidBodyStateEffector() + segment = 1 elif stateEffector == "linearTranslationBodiesOneDOF": stateEff, stateEffProps = setup_translatingBodiesOneDOF() segment = 1 @@ -370,8 +373,17 @@ def effectorBranchingIntegratedTest(show_plots, stateEffector, isParent, dynamic 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 + # Grab effector's inertial position + if stateEffector in ["hingedRigidBodies", "dualHingedRigidBodies", "nHingedRigidBodies"]: + r_ScN_N_log = np.zeros_like(inertialPropLog.r_BN_N) + for i in range(len(inertialPropLog.sigma_BN)): + sigmai_SN = inertialPropLog.sigma_BN[i, :] # MRP at timestep i and S is the parent frame not B + dcm_NS = np.transpose(rbk.MRP2C(sigmai_SN)) + r_ScN_N_log[i, :] = inertialPropLog.r_BN_N[i, :] + (dcm_NS @ stateEffProps.r_PcP_P).flatten() + else: + r_ScN_N_log = inertialPropLog.r_BN_N + + # Grab effector's attitude properties sigma_SN_log = inertialPropLog.sigma_BN # Compute conservation quantities using the state and dynamic effector's logged properties @@ -769,6 +781,36 @@ def setup_spinningBodiesNDOF(): return(spinningBodyEffector, stateEffProps) +def setup_hingedRigidBodyStateEffector(): + hingedBody = hingedRigidBodyStateEffector.HingedRigidBodyStateEffector() + + # Define properties of HRB + hingedBody.mass = 50.0 + hingedBody.IPntS_S = [[50.0, 0.0, 0.0], [0.0, 30.0, 0.0], [0.0, 0.0, 40.0]] + hingedBody.d = 1.0 + hingedBody.k = 100.0 + hingedBody.c = 50.0 + hingedBody.dcm_HB = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] + hingedBody.r_HB_B = [[0.5], [-1.5], [-0.5]] + hingedBody.thetaInit = 5 * macros.D2R + hingedBody.thetaDotInit = -1 * macros.D2R + hingedBody.ModelTag = "HingedRigidBody" + + # Compute COM offset contribution, to be divided by the hub mass + dcm_SH = rbk.euler2(hingedBody.thetaInit) + s1_hat = np.array([[-1.0],[0.0],[0.0]]) + mr_ScB_B = -(hingedBody.r_HB_B + np.transpose(hingedBody.dcm_HB) @ + (np.transpose(dcm_SH) @ (hingedBody.d * s1_hat))) * hingedBody.mass + + stateEffProps = stateEffectorProperties() + stateEffProps.totalMass = hingedBody.mass + stateEffProps.mr_PcB_B = mr_ScB_B + stateEffProps.r_PB_B = hingedBody.r_HB_B + stateEffProps.r_PcP_P = hingedBody.d * s1_hat + stateEffProps.inertialPropLogName = "hingedRigidBodyConfigLogOutMsg" + + return(hingedBody, stateEffProps) + def setup_translatingBodiesOneDOF(): translatingBody = linearTranslationOneDOFStateEffector.LinearTranslationOneDOFStateEffector() @@ -833,4 +875,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, "linearTranslationBodiesOneDOF", True, "extForceTorque", True) + effectorBranchingIntegratedTest(True, "hingedRigidBodies", True, "extForceTorque", True)