perform same adaptations to SB2DOF effector

Author: andrewmorell

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

@@ -56,6 +56,14 @@ SpinningBodyTwoDOFStateEffector::SpinningBodyTwoDOFStateEffector()
     this->nameOfTheta1DotState = "spinningBodyTheta1Dot" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID);
     this->nameOfTheta2State = "spinningBodyTheta2" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID);
     this->nameOfTheta2DotState = "spinningBodyTheta2Dot" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID);
+    this->nameOfInertialPositionProperty1 = "spinningBodyInertialPosition1" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID);
+    this->nameOfInertialVelocityProperty1 = "spinningBodyInertialVelocity1" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID);
+    this->nameOfInertialAttitudeProperty1 = "spinningBodyInertialAttitude1" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID);
+    this->nameOfInertialAngVelocityProperty1 = "spinningBodyInertialAngVelocity1" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID);
+    this->nameOfInertialPositionProperty2 = "spinningBodyInertialPosition2" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID);
+    this->nameOfInertialVelocityProperty2 = "spinningBodyInertialVelocity2" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID);
+    this->nameOfInertialAttitudeProperty2 = "spinningBodyInertialAttitude2" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID);
+    this->nameOfInertialAngVelocityProperty2 = "spinningBodyInertialAngVelocity2" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID);
     SpinningBodyTwoDOFStateEffector::effectorID++;
 }
 
@@ -111,10 +119,10 @@ void SpinningBodyTwoDOFStateEffector::writeOutputStateMessages(uint64_t CurrentC
         configLogMsg = this->spinningBodyConfigLogOutMsgs[0]->zeroMsgPayload;
 
         // Logging the S frame is the body frame B of that object
-        eigenVector3d2CArray(this->r_Sc1N_N, configLogMsg.r_BN_N);
-        eigenVector3d2CArray(this->v_Sc1N_N, configLogMsg.v_BN_N);
-        eigenVector3d2CArray(this->sigma_S1N, configLogMsg.sigma_BN);
-        eigenVector3d2CArray(this->omega_S1N_S1, configLogMsg.omega_BN_B);
+        eigenMatrixXd2CArray((*this->r_Sc1N_N), configLogMsg.r_BN_N);
+        eigenMatrixXd2CArray((*this->v_Sc1N_N), configLogMsg.v_BN_N);
+        eigenMatrixXd2CArray((*this->sigma_S1N), configLogMsg.sigma_BN);
+        eigenMatrixXd2CArray((*this->omega_S1N_S1), configLogMsg.omega_BN_B);
         this->spinningBodyConfigLogOutMsgs[0]->write(&configLogMsg, this->moduleID, CurrentClock);
     }
 
@@ -123,10 +131,10 @@ void SpinningBodyTwoDOFStateEffector::writeOutputStateMessages(uint64_t CurrentC
         configLogMsg = this->spinningBodyConfigLogOutMsgs[1]->zeroMsgPayload;
 
         // Logging the S frame is the body frame B of that object
-        eigenVector3d2CArray(this->r_Sc2N_N, configLogMsg.r_BN_N);
-        eigenVector3d2CArray(this->v_Sc2N_N, configLogMsg.v_BN_N);
-        eigenVector3d2CArray(this->sigma_S2N, configLogMsg.sigma_BN);
-        eigenVector3d2CArray(this->omega_S2N_S2, configLogMsg.omega_BN_B);
+        eigenMatrixXd2CArray(*this->r_Sc2N_N, configLogMsg.r_BN_N);
+        eigenMatrixXd2CArray(*this->v_Sc2N_N, configLogMsg.v_BN_N);
+        eigenMatrixXd2CArray(*this->sigma_S2N, configLogMsg.sigma_BN);
+        eigenMatrixXd2CArray(*this->omega_S2N_S2, configLogMsg.omega_BN_B);
         this->spinningBodyConfigLogOutMsgs[1]->write(&configLogMsg, this->moduleID, CurrentClock);
     }
 }
@@ -167,6 +175,35 @@ void SpinningBodyTwoDOFStateEffector::registerStates(DynParamManager& states)
     thetaDotInitMatrix(1,0) = this->theta2DotInit;
     this->theta1DotState->setState(thetaDotInitMatrix.row(0));
     this->theta2DotState->setState(thetaDotInitMatrix.row(1));
+
+    registerProperties(states);
+}
+
+void SpinningBodyTwoDOFStateEffector::addDynamicEffector(DynamicEffector *newDynamicEffector, int segment)
+{
+    this->assignStateParamNames<DynamicEffector *>(newDynamicEffector, segment);
+
+    this->dynEffectors.push_back(newDynamicEffector);
+}
+
+void SpinningBodyTwoDOFStateEffector::registerProperties(DynParamManager& states)
+{
+    Eigen::Vector3d stateInit = Eigen::Vector3d::Zero();
+    this->r_Sc1N_N = states.createProperty(this->nameOfInertialPositionProperty1, stateInit);
+    this->v_Sc1N_N = states.createProperty(this->nameOfInertialVelocityProperty1, stateInit);
+    this->sigma_S1N = states.createProperty(this->nameOfInertialAttitudeProperty1, stateInit);
+    this->omega_S1N_S1 = states.createProperty(this->nameOfInertialAngVelocityProperty1, stateInit);
+
+    this->r_Sc2N_N = states.createProperty(this->nameOfInertialPositionProperty2, stateInit);
+    this->v_Sc2N_N = states.createProperty(this->nameOfInertialVelocityProperty2, stateInit);
+    this->sigma_S2N = states.createProperty(this->nameOfInertialAttitudeProperty2, stateInit);
+    this->omega_S2N_S2 = states.createProperty(this->nameOfInertialAngVelocityProperty2, stateInit);
+
+    std::vector<DynamicEffector*>::iterator dynIt;
+    for(dynIt = this->dynEffectors.begin(); dynIt != this->dynEffectors.end(); dynIt++)
+    {
+        (*dynIt)->linkInProperties(states);
+    }
 }
 
 /*! This method allows the SB state effector to provide its contributions to the mass props and mass prop rates of the
@@ -271,6 +308,26 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back
     gLocal_N = g_N;
     g_B = this->dcm_BN * gLocal_N;
 
+    // Loop through to collect forces and torques from any connected dynamic effectors
+    Eigen::Vector3d attBodyForce_S1 = Eigen::Vector3d::Zero();
+    Eigen::Vector3d attBodyTorquePntS1_S1 = Eigen::Vector3d::Zero();
+    Eigen::Vector3d attBodyForce_S2 = Eigen::Vector3d::Zero();
+    Eigen::Vector3d attBodyTorquePntS2_S2 = Eigen::Vector3d::Zero();
+    std::vector<DynamicEffector*>::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));
+        if ((*dynIt)->getPropName_inertialPosition() == this->nameOfInertialPositionProperty1) {
+            attBodyForce_S1 += (*dynIt)->forceExternal_B;
+            attBodyTorquePntS1_S1 += (*dynIt)->torqueExternalPntB_B;
+        }
+        else if ((*dynIt)->getPropName_inertialPosition() == this->nameOfInertialPositionProperty2) {
+            attBodyForce_S2 += (*dynIt)->forceExternal_B;
+            attBodyTorquePntS2_S2 += (*dynIt)->torqueExternalPntB_B;
+        }
+    }
+
     // Update omega_BN_B
     this->omega_BN_B = omega_BN_B;
     this->omegaTilde_BN_B = eigenTilde(this->omega_BN_B);
@@ -334,12 +391,15 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back
         + this->mass1 * (rTilde_Sc1S1_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * rTilde_Sc1S1_B) * this->rPrime_Sc1S1_B
         + this->mass2 * (rTilde_Sc2S1_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * rTilde_Sc2S1_B) * this->rPrime_Sc2S1_B
         + this->mass2 * rTilde_Sc2S1_B * omegaTilde_S2S1_B * this->rPrime_Sc2S2_B
-        + this->mass * rTilde_ScS1_B * this->omegaTilde_BN_B * rDot_S1B_B);
+        + this->mass * rTilde_ScS1_B * this->omegaTilde_BN_B * rDot_S1B_B
+        - this->dcm_BS1 * attBodyTorquePntS1_S1 + this->dcm_BS2 * attBodyTorquePntS2_S2
+        - eigenTilde(this->r_S2S1_B) * (this->dcm_BS2 * attBodyForce_S2));
     CThetaStar(1, 0) = this->u2 - this->k2 * (this->theta2 - this->theta2Ref)
         - this->c2 * (this->theta2Dot - this->theta2DotRef) + this->s2Hat_B.transpose() * gravityTorquePntS2_B
         - this->s2Hat_B.transpose() * ((IPrimeS2PntS2_B + this->omegaTilde_BN_B * IS2PntS2_B) * this->omega_S2N_B
         + IS2PntS2_B * this->omegaTilde_S1B_B * this->omega_S2S1_B + this->mass2 * rTilde_Sc2S2_B * omegaTilde_S1N_B * this->rPrime_S2S1_B
-        + this->mass2 * rTilde_Sc2S2_B * this->omegaTilde_BN_B * (rDot_S2S1_B + rDot_S1B_B));
+        + this->mass2 * rTilde_Sc2S2_B * this->omegaTilde_BN_B * (rDot_S2S1_B + rDot_S1B_B)
+        - this->dcm_BS2 * attBodyTorquePntS2_S2);
 
     // Check if any of the axis are locked and change dynamics accordingly
     if (this->lockFlag1 == 1 || this->lockFlag2 == 1)
@@ -373,7 +433,8 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back
     backSubContr.matrixA = - this->mass * rTilde_ScS1_B * this->s1Hat_B * this->ATheta.row(0) - this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->ATheta.row(1);
     backSubContr.matrixB = - this->mass * rTilde_ScS1_B * this->s1Hat_B * this->BTheta.row(0) - this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->BTheta.row(1);
     backSubContr.vecTrans = - this->mass * this->omegaTilde_S1B_B * this->rPrime_ScB_B - this->mass2 * (omegaTilde_S2S1_B * this->rPrime_Sc2S2_B - rTilde_Sc2S2_B * this->omegaTilde_S1B_B * this->omega_S2S1_B)
-        + this->mass * rTilde_ScS1_B * this->s1Hat_B * this->CTheta.row(0) + this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->CTheta.row(1);
+        + this->mass * rTilde_ScS1_B * this->s1Hat_B * this->CTheta.row(0) + this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->CTheta.row(1)
+        + this->dcm_BS1 * attBodyForce_S1 + this->dcm_BS2 * attBodyForce_S2;
 
     // Rotation contributions
     backSubContr.matrixC = (this->IS1PntSc1_B + this->IS2PntSc2_B - this->mass1 * this->rTilde_Sc1B_B * rTilde_Sc1S1_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S1_B) * this->s1Hat_B * this->ATheta.row(0)
@@ -386,7 +447,10 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back
         - this->mass2 * (this->rTilde_Sc2B_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * this->rTilde_Sc2B_B) * this->rPrime_Sc2B_B
         - this->mass2 * this->rTilde_Sc2B_B * omegaTilde_S2S1_B * this->rPrime_Sc2S2_B
         - (this->IS1PntSc1_B + this->IS2PntSc2_B - this->mass1 * this->rTilde_Sc1B_B * rTilde_Sc1S1_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S1_B) * this->s1Hat_B * this->CTheta.row(0)
-        - (this->IS2PntSc2_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S2_B) * this->s2Hat_B * this->CTheta.row(1);
+        - (this->IS2PntSc2_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S2_B) * this->s2Hat_B * this->CTheta.row(1)
+        + this->dcm_BS1 * attBodyTorquePntS1_S1 + this->dcm_BS2 * attBodyTorquePntS2_S2
+        + eigenTilde(this->r_S1B_B) * (this->dcm_BS1 * attBodyForce_S1)
+        + eigenTilde(this->r_S2S1_B + this->r_S1B_B) * (this->dcm_BS2 * attBodyForce_S2);
 }
 
 /*! This method is used to find the derivatives for the SB stateEffector: thetaDDot and the kinematic derivative */
@@ -439,20 +503,20 @@ void SpinningBodyTwoDOFStateEffector::computeSpinningBodyInertialStates()
     Eigen::Matrix3d dcm_S2N;
     dcm_S1N = (this->dcm_BS1).transpose() * this->dcm_BN;
     dcm_S2N = (this->dcm_BS2).transpose() * this->dcm_BN;
-    this->sigma_S1N = eigenMRPd2Vector3d(eigenC2MRP(dcm_S1N));
-    this->sigma_S2N = eigenMRPd2Vector3d(eigenC2MRP(dcm_S2N));
+    *this->sigma_S1N = eigenMRPd2Vector3d(eigenC2MRP(dcm_S1N));
+    *this->sigma_S2N = eigenMRPd2Vector3d(eigenC2MRP(dcm_S2N));
 
     // Convert the angular velocity to the corresponding frame
-    this->omega_S1N_S1 = dcm_BS1.transpose() * this->omega_S1N_B;
-    this->omega_S2N_S2 = dcm_BS2.transpose() * this->omega_S2N_B;
+    *this->omega_S1N_S1 = dcm_BS1.transpose() * this->omega_S1N_B;
+    *this->omega_S2N_S2 = dcm_BS2.transpose() * this->omega_S2N_B;
 
     // Compute the inertial position vector
-    this->r_Sc1N_N = (Eigen::Vector3d)(*this->inertialPositionProperty) + this->dcm_BN.transpose() * this->r_Sc1B_B;
-    this->r_Sc2N_N = (Eigen::Vector3d)(*this->inertialPositionProperty) + this->dcm_BN.transpose() * this->r_Sc2B_B;
+    *this->r_Sc1N_N = (Eigen::Vector3d)(*this->inertialPositionProperty) + this->dcm_BN.transpose() * this->r_Sc1B_B;
+    *this->r_Sc2N_N = (Eigen::Vector3d)(*this->inertialPositionProperty) + this->dcm_BN.transpose() * this->r_Sc2B_B;
 
     // Compute the inertial velocity vector
-    this->v_Sc1N_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) + this->dcm_BN.transpose() * this->rDot_Sc1B_B;
-    this->v_Sc2N_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) + this->dcm_BN.transpose() * this->rDot_Sc2B_B;
+    *this->v_Sc1N_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) + this->dcm_BN.transpose() * this->rDot_Sc1B_B;
+    *this->v_Sc2N_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) + this->dcm_BN.transpose() * this->rDot_Sc2B_B;
 }
 
 /*! This method is used so that the simulation will ask SB to update messages */