[drop] debugging nans in integrated test

Author: DavilaDawg

src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.cpp

@@ -45,6 +45,45 @@ HingedRigidBodyStateEffector::HingedRigidBodyStateEffector()
     this->r_HB_B.setZero();
     this->dcm_HB.Identity();
 
+    // Defensive zeroing of all internal members
+    this->r_BP_P.setZero();
+    this->r_HP_P.setZero();
+    this->r_SP_P.setZero();
+    this->rPrime_SP_P.setZero();
+
+    this->omegaLoc_PN_P.setZero();
+    this->omega_PN_S.setZero();
+    this->sHat1_P.setZero();
+    this->sHat2_P.setZero();
+    this->sHat3_P.setZero();
+
+    this->rTilde_HP_P.setZero();
+    this->rTilde_SP_P.setZero();
+    this->rPrimeTilde_SP_P.setZero();
+    this->ISPrimePntS_P.setZero();
+    this->omegaTildeLoc_PN_P.setZero();
+
+    this->dcm_BP.setIdentity();
+    this->dcm_HP.setIdentity();
+    this->dcm_SH.setIdentity();
+    this->dcm_SP.setIdentity();
+
+    this->aTheta.setZero();
+    this->bTheta.setZero();
+    this->cTheta = 0.0;
+
+    // Null all pointers until registered/linked
+    this->r_SN_N = nullptr;
+    this->v_SN_N = nullptr;
+    this->sigma_SN = nullptr;
+    this->omega_SN_S = nullptr;
+    this->inertialPositionProperty = nullptr;
+    this->inertialVelocityProperty = nullptr;
+    this->c_B = nullptr;
+    this->cPrime_B = nullptr;
+    this->thetaState = nullptr;
+    this->thetaDotState = nullptr;
+
     this->nameOfThetaState = "hingedRigidBodyTheta" + std::to_string(this->effectorID);
     this->nameOfThetaDotState = "hingedRigidBodyThetaDot" + std::to_string(this->effectorID);
     this->nameOfInertialPositionProperty = "hingedRigidBodyInertialPosition" + std::to_string(this->effectorID);

src/simulation/dynamics/_GeneralModuleFiles/_UnitTestDynamics/test_effectorBranching_integrated.py

@@ -241,6 +241,7 @@ def effectorBranchingIntegratedTest(show_plots, stateEffector, isParent, dynamic
     scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]]
     scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]]
     scObject.hub.omega_BN_BInit = [[0.1], [0.1], [0.1]]
+    scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]]
 
     # Add Earth gravity to the simulation
     earthGravBody = gravityEffector.GravBodyData()
@@ -365,6 +366,7 @@ def effectorBranchingIntegratedTest(show_plots, stateEffector, isParent, dynamic
     # Grab effector's inertial position
     if stateEffector in ["hingedRigidBodies", "dualHingedRigidBodies", "nHingedRigidBodies"]:
         r_ScN_N_log = np.zeros_like(inertialPropLog.r_BN_N)
+        print("sigma_BN: ", inertialPropLog.sigma_BN)
         for i in range(len(inertialPropLog.sigma_BN)):
             q_i = inertialPropLog.sigma_BN[i, :]  # MRP at timestep i
             dcm_NP = np.transpose(rbk.MRP2C(q_i))
@@ -703,6 +705,11 @@ def setup_hingedRigidBodyStateEffector():
     stateEffProps.r_PcP_P = hingedBody.d * np.array([[-1.0],[0.0],[0.0]])
     stateEffProps.inertialPropLogName = "hingedRigidBodyConfigLogOutMsg"
 
+    print("mr_PcB_B: ", stateEffProps.mr_PcB_B)
+    print("r_PB_B: ", stateEffProps.r_PB_B)
+    print("r_PcP_P: ", stateEffProps.r_PcP_P)
+    print("hingedBody.mass: ", stateEffProps.totalMass)
+
     return(hingedBody, stateEffProps)
 
 def setup_linearSpringMassDamper():