Merge pull request #514 from AVSLab/bug/spinning-bodies-initialization

Bug/spinning bodies initialization

src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp

@@ -34,10 +34,16 @@ SpinningBodyOneDOFStateEffector::SpinningBodyOneDOFStateEffector()
 
     // Initialize variables to working values
     this->IPntSc_S.setIdentity();
-    this->dcm_BS.setIdentity();
     this->dcm_S0B.setIdentity();
     this->r_SB_B.setZero();
     this->sHat_S.setZero();
+    this->dcm_BS.setIdentity();
+    this->rTilde_ScB_B.setZero();
+    this->omegaTilde_SB_B.setZero();
+    this->omegaTilde_BN_B.setZero();
+    this->dcm_BS.setIdentity();
+    this->dcm_BN.setIdentity();
+    this->IPntSc_B.setIdentity();
 
     this->nameOfThetaState = "spinningBodyTheta" + std::to_string(SpinningBodyOneDOFStateEffector::effectorID);
     this->nameOfThetaDotState = "spinningBodyThetaDot" + std::to_string(SpinningBodyOneDOFStateEffector::effectorID);

src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h

@@ -45,9 +45,9 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel {
     double thetaDotInit = 0.0;                                       //!< [rad/s] initial spinning body angle rate
     std::string nameOfThetaState;                                    //!< -- identifier for the theta state data container
     std::string nameOfThetaDotState;                                 //!< -- identifier for the thetaDot state data container
-    Eigen::Vector3d r_SB_B;                                          //!< [m] vector pointing from body frame B origin to spinning frame S origin in B frame components
-    Eigen::Vector3d r_ScS_S;                                         //!< [m] vector pointing from spinning frame S origin to point Sc (center of mass of the spinner) in S frame components
-    Eigen::Vector3d sHat_S;                                          //!< -- spinning axis in S frame components.
+    Eigen::Vector3d r_SB_B{0.0, 0.0, 0.0};                  //!< [m] vector pointing from body frame B origin to spinning frame S origin in B frame components
+    Eigen::Vector3d r_ScS_S{0.0, 0.0, 0.0};                 //!< [m] vector pointing from spinning frame S origin to point Sc (center of mass of the spinner) in S frame components
+    Eigen::Vector3d sHat_S{1.0, 0.0, 0.0};                  //!< -- spinning axis in S frame components.
     Eigen::Matrix3d IPntSc_S;                                        //!< [kg-m^2] Inertia of spinning body about point Sc in S frame components
     Eigen::Matrix3d dcm_S0B;                                         //!< -- DCM from the body frame to the S0 frame (S frame for theta=0)
     Message<HingedRigidBodyMsgPayload> spinningBodyOutMsg;           //!< state output message
@@ -63,10 +63,14 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel {
     void UpdateState(uint64_t CurrentSimNanos) override;             //!< -- Method for updating information
     void registerStates(DynParamManager& statesIn) override;         //!< -- Method for registering the SB states
     void linkInStates(DynParamManager& states) override;             //!< -- 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) override;   //!< -- Method for back-substitution contributions
-    void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) override;                         //!< -- Method for SB to compute its derivatives
+    void updateContributions(double integTime,
+                             BackSubMatrices& backSubContr, Eigen::Vector3d sigma_BN,
+                             Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) override;   //!< -- Method for back-substitution contributions
+    void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N,
+                            Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) override;                         //!< -- Method for SB 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;         //!< -- Method for computing energy and momentum for SBs
+    void updateEnergyMomContributions(double integTime, Eigen::Vector3d& rotAngMomPntCContr_B,
+                                      double& rotEnergyContr, Eigen::Vector3d omega_BN_B) override;         //!< -- Method for computing energy and momentum for SBs
     void prependSpacecraftNameToStates() override;                   //!< Method used for multiple spacecraft
     void computeSpinningBodyInertialStates();               //!< Method for computing the SB's states
 
@@ -78,22 +82,22 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel {
     double thetaDotRef = 0.0;           //!< [rad] spinning body reference angle rate
 
     // Terms needed for back substitution
-    Eigen::Vector3d aTheta;             //!< -- rDDot_BN term for back substitution
-    Eigen::Vector3d bTheta;             //!< -- omegaDot_BN term for back substitution
-    double cTheta = 0.0;                //!< -- scalar term for back substitution
-    double mTheta = 0.0;                //!< -- auxiliary term for back substitution
+    Eigen::Vector3d aTheta{0.0, 0.0, 0.0};             //!< -- rDDot_BN term for back substitution
+    Eigen::Vector3d bTheta{0.0, 0.0, 0.0};             //!< -- omegaDot_BN term for back substitution
+    double cTheta = 0.0;                                        //!< -- scalar term for back substitution
+    double mTheta = 0.0;                                        //!< -- auxiliary term for back substitution
 
     // Vector quantities
-    Eigen::Vector3d sHat_B;             //!< -- spinning axis in B frame components
-    Eigen::Vector3d r_ScS_B;            //!< [m] vector pointing from spinning frame S origin to point Sc in B frame components
-    Eigen::Vector3d r_ScB_B;            //!< [m] vector pointing from body frame B origin to point Sc in B frame components.
-    Eigen::Vector3d rPrime_ScS_B;       //!< [m/s] body frame time derivative of r_ScS_B
-    Eigen::Vector3d rPrime_ScB_B;       //!< [m/s] body frame time derivative of r_ScB_B
-    Eigen::Vector3d rDot_ScB_B;         //!< [m/s] inertial frame time derivative of r_ScB_B
-    Eigen::Vector3d omega_SB_B;         //!< [rad/s] angular velocity of the S frame wrt the B frame in B frame components.
-    Eigen::Vector3d omega_BN_B;         //!< [rad/s] angular velocity of the B frame wrt the N frame in B frame components.
-    Eigen::Vector3d omega_SN_B;         //!< [rad/s] angular velocity of the S frame wrt the N frame in B frame components.
-    Eigen::MRPd sigma_BN;               //!< -- body frame attitude wrt to the N frame in MRPs
+    Eigen::Vector3d sHat_B{1.0, 0.0, 0.0};             //!< -- spinning axis in B frame components
+    Eigen::Vector3d r_ScS_B{0.0, 0.0, 0.0};            //!< [m] vector pointing from spinning frame S origin to point Sc in B frame components
+    Eigen::Vector3d r_ScB_B{0.0, 0.0, 0.0};            //!< [m] vector pointing from body frame B origin to point Sc in B frame components.
+    Eigen::Vector3d rPrime_ScS_B{0.0, 0.0, 0.0};       //!< [m/s] body frame time derivative of r_ScS_B
+    Eigen::Vector3d rPrime_ScB_B{0.0, 0.0, 0.0};       //!< [m/s] body frame time derivative of r_ScB_B
+    Eigen::Vector3d rDot_ScB_B{0.0, 0.0, 0.0};         //!< [m/s] inertial frame time derivative of r_ScB_B
+    Eigen::Vector3d omega_SB_B{0.0, 0.0, 0.0};         //!< [rad/s] angular velocity of the S frame wrt the B frame in B frame components.
+    Eigen::Vector3d omega_BN_B{0.0, 0.0, 0.0};         //!< [rad/s] angular velocity of the B frame wrt the N frame in B frame components.
+    Eigen::Vector3d omega_SN_B{0.0, 0.0, 0.0};         //!< [rad/s] angular velocity of the S frame wrt the N frame in B frame components.
+    Eigen::MRPd sigma_BN{0.0, 0.0, 0.0};               //!< -- body frame attitude wrt to the N frame in MRPs
 
     // Matrix quantities
     Eigen::Matrix3d rTilde_ScB_B;       //!< [m] tilde matrix of r_ScB_B
@@ -104,10 +108,10 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel {
     Eigen::Matrix3d IPntSc_B;           //!< [kg-m^2] inertia of spinning body about point Sc in B frame components
 
     // Spinning body properties
-    Eigen::Vector3d r_ScN_N;            //!< [m] position vector of spinning body center of mass Sc relative to the inertial frame origin N
-    Eigen::Vector3d v_ScN_N;            //!< [m/s] inertial velocity vector of Sc relative to inertial frame
-    Eigen::Vector3d sigma_SN;           //!< -- MRP attitude of frame S relative to inertial frame
-    Eigen::Vector3d omega_SN_S;         //!< [rad/s] inertial spinning body frame angular velocity vector
+    Eigen::Vector3d r_ScN_N{0.0, 0.0, 0.0};            //!< [m] position vector of spinning body center of mass Sc relative to the inertial frame origin N
+    Eigen::Vector3d v_ScN_N{0.0, 0.0, 0.0};            //!< [m/s] inertial velocity vector of Sc relative to inertial frame
+    Eigen::Vector3d sigma_SN{0.0, 0.0, 0.0};           //!< -- MRP attitude of frame S relative to inertial frame
+    Eigen::Vector3d omega_SN_S{0.0, 0.0, 0.0};         //!< [rad/s] inertial spinning body frame angular velocity vector
 
     // States
     double theta = 0.0;                           //!< [rad] spinning body angle

src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp

@@ -37,10 +37,20 @@ SpinningBodyTwoDOFStateEffector::SpinningBodyTwoDOFStateEffector()
     this->IS2PntSc2_S2.setIdentity();
     this->dcm_S10B.setIdentity();
     this->dcm_S20S1.setIdentity();
-    this->r_S1B_B.setZero();
-    this->r_S2S1_S1.setZero();
-    this->s1Hat_S1.setZero();
-    this->s2Hat_S2.setZero();
+    this->ATheta.setZero();
+    this->BTheta.setZero();
+    this->rTilde_Sc1B_B.setZero();
+    this->rTilde_Sc2B_B.setZero();
+    this->omegaTilde_S1B_B.setZero();
+    this->omegaTilde_S2B_B.setZero();
+    this->omegaTilde_BN_B.setZero();
+    this->dcm_BS1.setIdentity();
+    this->dcm_BS2.setIdentity();
+    this->dcm_BN.setIdentity();
+    this->IS1PntSc1_B.setIdentity();
+    this->IS2PntSc2_B.setIdentity();
+    this->IPrimeS1PntSc1_B.setZero();
+    this->IPrimeS2PntSc2_B.setZero();
 
     this->nameOfTheta1State = "spinningBodyTheta1" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID);
     this->nameOfTheta1DotState = "spinningBodyTheta1Dot" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID);