diff --git a/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.cpp b/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.cpp index 0d3691b2f1c..a70fa73fc52 100644 --- a/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.cpp +++ b/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.cpp @@ -78,16 +78,33 @@ LinearSpringMassDamper::~LinearSpringMassDamper() */ void LinearSpringMassDamper::linkInStates(DynParamManager& states) { - // - Grab access to gravity - this->g_N = states.getPropertyReference(this->propName_vehicleGravity); - // - Grab access to c_B and cPrime_B - this->c_B = states.getPropertyReference(this->propName_centerOfMassSC); - this->cPrime_B = states.getPropertyReference(this->propName_centerOfMassPrimeSC); + // Get access to the hub's states needed for dynamic coupling + this->hubOmega = states.getStateObject("hubOmega"); + + const std::string gravName = this->propName_vehicleGravity.empty() ? "g_N" : this->propName_vehicleGravity; + this->g_N = states.getPropertyReference(gravName); + + if (!this->propName_centerOfMassSC.empty()) { + this->c_B = states.getPropertyReference(this->propName_centerOfMassSC); + } + if (!this->propName_centerOfMassPrimeSC.empty()) { + this->cPrime_B = states.getPropertyReference(this->propName_centerOfMassPrimeSC); + } return; } +void LinearSpringMassDamper::linkInPrescribedMotionProperties(DynParamManager& states) +{ + this->prescribedPositionProperty = states.getPropertyReference(this->propName_prescribedPosition); + this->prescribedVelocityProperty = states.getPropertyReference(this->propName_prescribedVelocity); + this->prescribedAccelerationProperty = states.getPropertyReference(this->propName_prescribedAcceleration); + this->prescribedAttitudeProperty = states.getPropertyReference(this->propName_prescribedAttitude); + this->prescribedAngVelocityProperty = states.getPropertyReference(this->propName_prescribedAngVelocity); + this->prescribedAngAccelerationProperty = states.getPropertyReference(this->propName_prescribedAngAcceleration); +} + /*! This is the method for the spring mass damper particle to register its states: rho and rhoDot * * @param[in,out] states Dynamic parameter manager used to register states or properties. @@ -115,6 +132,8 @@ void LinearSpringMassDamper::registerStates(DynParamManager& states) return; } +// Create method linkInPrescribedMotionProperties + /*! This is the method for the SMD to add its contributions to the mass props and mass prop rates of the vehicle * * @param[in] integTime [s] Current integration time. @@ -281,6 +300,62 @@ void LinearSpringMassDamper::computeDerivatives(double integTime [[maybe_unused] return; } +void LinearSpringMassDamper::addPrescribedMotionCouplingContributions(BackSubMatrices& backSubContr) +{ + // Access prescribed motion properties + Eigen::Vector3d r_PB_B = (Eigen::Vector3d) * this->prescribedPositionProperty; + Eigen::Vector3d rPrime_PB_B = (Eigen::Vector3d) * this->prescribedVelocityProperty; + Eigen::Vector3d rPrimePrime_PB_B = (Eigen::Vector3d) * this->prescribedAccelerationProperty; + Eigen::MRPd sigma_PB(this->prescribedAttitudeProperty->data()); + Eigen::Vector3d omega_PB_P = (Eigen::Vector3d) * this->prescribedAngVelocityProperty; + Eigen::Vector3d omegaPrime_PB_P = (Eigen::Vector3d) * this->prescribedAngAccelerationProperty; + Eigen::Matrix3d dcm_PB = sigma_PB.toRotationMatrix().transpose(); + + // Collect hub states + Eigen::Vector3d omega_BN_B = this->hubOmega->stateView(); + Eigen::Vector3d omega_BN_P = dcm_PB * omega_BN_B; + + // Prescribed motion coupling contributions + Eigen::Vector3d tHat_P = this->pHat_B; + Eigen::Vector3d r_PB_P = dcm_PB * r_PB_B; + Eigen::Matrix3d rTilde_PB_P = eigenTilde(r_PB_P); + backSubContr.matrixB += -this->massSMD * tHat_P * this->aRho.transpose() * rTilde_PB_P; + + Eigen::Matrix3d omegaTilde_PB_P = eigenTilde(omega_PB_P); + Eigen::Vector3d rPPrime_TB_B = this->rPrime_PcB_B; + Eigen::Matrix3d omegaPrimeTilde_PB_P = eigenTilde(omegaPrime_PB_P); + Eigen::Vector3d r_TB_P = this->r_PcB_B; + Eigen::Vector3d rPrimePrime_PB_P = dcm_PB * rPrimePrime_PB_B; + Eigen::Matrix3d omegaTilde_BN_P = eigenTilde(omega_BN_P); + Eigen::Vector3d rPrime_PB_P = dcm_PB * rPrime_PB_B; + Eigen::Vector3d term1 = 2.0 * omegaTilde_PB_P * rPPrime_TB_B + omegaPrimeTilde_PB_P * r_TB_P + + omegaTilde_PB_P * omegaTilde_PB_P * r_TB_P + rPrimePrime_PB_P; + Eigen::Vector3d term2 = + rPrimePrime_PB_P + 2.0 * omegaTilde_BN_P * rPrime_PB_P + omegaTilde_BN_P * omegaTilde_BN_P * r_PB_P; + Eigen::Vector3d term3 = omegaPrime_PB_P + omegaTilde_BN_P * omega_PB_P; + backSubContr.vecTrans += -this->massSMD * term1 - this->massSMD * this->aRho.transpose() * term2 * tHat_P - + this->massSMD * this->bRho.transpose() * term3 * tHat_P; + + // Prescribed motion rotation coupling contributions + backSubContr.matrixC += this->massSMD * rTilde_PB_P * tHat_P * this->aRho.transpose(); + + Eigen::Vector3d r_FcB_P = r_TB_P + r_PB_P; + Eigen::Matrix3d rTilde_FcB_P = eigenTilde(r_FcB_P); + backSubContr.matrixD += +this->massSMD * rTilde_PB_P * tHat_P * this->bRho.transpose() - + this->massSMD * rTilde_FcB_P * tHat_P * this->aRho.transpose() * rTilde_PB_P; + + Eigen::Matrix3d rTilde_FcP_P = eigenTilde(r_TB_P); + + Eigen::Vector3d vecRotTerm2 = -this->massSMD * rTilde_FcB_P * term1; + Eigen::Vector3d vecRotTerm3 = + -this->massSMD * (omegaTilde_BN_P * rTilde_PB_P - omegaTilde_PB_P * rTilde_FcP_P) * rPPrime_TB_B - + this->massSMD * omegaTilde_BN_P * rTilde_FcB_P * (omegaTilde_PB_P * r_TB_P + rPrime_PB_P); + Eigen::Vector3d vecRotTerm4 = -this->massSMD * this->cRho * rTilde_PB_P * tHat_P; + Eigen::Vector3d vecRotTerm5 = -this->massSMD * rTilde_FcB_P * tHat_P * (this->aRho.transpose() * term2) - + this->massSMD * rTilde_FcB_P * tHat_P * (this->bRho.transpose() * term3); + backSubContr.vecRot += vecRotTerm2 + vecRotTerm3 + vecRotTerm4 + vecRotTerm5; +} + /*! This method is for the SMD to add its contributions to energy and momentum * * @param[in] integTime [s] Current integration time. diff --git a/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.h b/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.h index fb426aa453d..c0cf14cbd66 100644 --- a/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.h +++ b/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.h @@ -76,6 +76,8 @@ class LinearSpringMassDamper final void Reset(uint64_t CurrentSimNanos) override; void registerStates(DynParamManager& states) override; //!< Method for SMD to register its states void linkInStates(DynParamManager& states) override; //!< Method for SMD to get access of other states + void linkInPrescribedMotionProperties( + DynParamManager& states) override; //!< Method for SMD to get access to prescribed motion properties void retrieveMassValue(double integTime) override; void calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B) override; //!< Force and torque on s/c due to linear spring mass damper @@ -93,6 +95,11 @@ class LinearSpringMassDamper final Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::MRPd sigma_BN) override; //!< Method for each stateEffector to calculate derivatives + void addPrescribedMotionCouplingContributions(BackSubMatrices& backSubContr) + override; //!< Method for adding coupling contributions for state effector branching on prescribed motion + + // Properties required for prescribed motion branching/attachment + StateData* hubOmega; //!< [rad/s] hub inertial angular velocity vector }; #endif /* LINEAR_SPRING_MASS_DAMPER_H */ diff --git a/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffectorBranching.py b/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffectorBranching.py index c132aaa20b2..81e26bc811c 100644 --- a/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffectorBranching.py +++ b/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffectorBranching.py @@ -35,6 +35,7 @@ from Basilisk.simulation import prescribedLinearTranslation from Basilisk.simulation import prescribedRotation1DOF from Basilisk.simulation import linearTranslationOneDOFStateEffector +from Basilisk.simulation import linearSpringMassDamper from Basilisk.simulation import spinningBodyOneDOFStateEffector from Basilisk.simulation import spinningBodyTwoDOFStateEffector from Basilisk.architecture import messaging @@ -609,6 +610,123 @@ def test_prescribed_branching_linear_translation_one_dof(show_plots): unit_test_check_verification(orb_ang_mom_n, orb_energy, rot_ang_mom_n) +def test_prescribed_branching_linear_spring_mass_damper(show_plots): + r""" + **Verification Test Description** + + This unit test checks the branching capability of the prescribed motion module by connecting a + :ref:`linearSpringMassDamper` to the prescribed motion state effector. Both the + :ref:`prescribedRotation1DOF` and :ref:`prescribedLinearTranslation` modules are configured so that + the prescribed body simultaneously translates and rotates relative to the spacecraft hub. The particle + is set up to have free (undamped) spring motion along its direction of translation. + + **Description of Variables Being Tested** + + The test checks that the conservation quantities of the spacecraft orbital energy, orbital angular momentum, + and rotational angular momentum remain constant over the duration of the simulation. + """ + + task_name = "unitTask" + process_name = "testProcess" + sim = SimulationBaseClass.SimBaseClass() + process_rate = macros.sec2nano(0.001) + process = sim.CreateNewProcess(process_name) + process.addTask(sim.CreateNewTask(task_name, process_rate)) + + # Create the spacecraft + sc_object = create_spacecraft_hub() + sim.AddModelToTask(task_name, sc_object) + + # Create prescribed motion object + prescribed_platform = create_prescribed_body(r_P0M_M, sigma_P0M) + sim.AddModelToTask(task_name, prescribed_platform) + sc_object.addStateEffector(prescribed_platform) + + # Create rotational motion profiler + one_dof_rotation_profiler = prescribedRotation1DOF.PrescribedRotation1DOF() + one_dof_rotation_profiler.ModelTag = "prescribedRotation1DOF" + one_dof_rotation_profiler.setRotHat_M(prescribed_rot_axis_M) + one_dof_rotation_profiler.setThetaDDotMax(prescribed_ang_accel_max) + one_dof_rotation_profiler.setThetaInit(prescribed_theta_init) + one_dof_rotation_profiler.setCoastOptionBangDuration(1.0) + one_dof_rotation_profiler.setSmoothingDuration(1.0) + sim.AddModelToTask(task_name, one_dof_rotation_profiler) + + prescribed_rotation_msg_data = messaging.HingedRigidBodyMsgPayload() + prescribed_rotation_msg_data.theta = prescribed_theta_ref + prescribed_rotation_msg_data.thetaDot = 0.0 + prescribed_rotation_msg = messaging.HingedRigidBodyMsg().write(prescribed_rotation_msg_data) + one_dof_rotation_profiler.spinningBodyInMsg.subscribeTo(prescribed_rotation_msg) + prescribed_platform.prescribedRotationInMsg.subscribeTo(one_dof_rotation_profiler.prescribedRotationOutMsg) + + # Create translational motion profiler + one_dof_translation_profiler = prescribedLinearTranslation.PrescribedLinearTranslation() + one_dof_translation_profiler.ModelTag = "prescribedLinearTranslation" + one_dof_translation_profiler.setTransHat_M(prescribed_trans_axis_M) + one_dof_translation_profiler.setTransAccelMax(prescribed_trans_accel_max) + one_dof_translation_profiler.setTransPosInit(prescribed_pos_init) + one_dof_translation_profiler.setCoastOptionBangDuration(1.0) + one_dof_translation_profiler.setSmoothingDuration(1.0) + sim.AddModelToTask(task_name, one_dof_translation_profiler) + + prescribed_translation_msg_data = messaging.LinearTranslationRigidBodyMsgPayload() + prescribed_translation_msg_data.rho = prescribed_pos_ref + prescribed_translation_msg_data.rhoDot = 0.0 + prescribed_translation_msg = messaging.LinearTranslationRigidBodyMsg().write(prescribed_translation_msg_data) + one_dof_translation_profiler.linearTranslationRigidBodyInMsg.subscribeTo(prescribed_translation_msg) + prescribed_platform.prescribedTranslationInMsg.subscribeTo(one_dof_translation_profiler.prescribedTranslationOutMsg) + + # Create the spring-mass-damper particle + smd = linearSpringMassDamper.LinearSpringMassDamper() + smd.ModelTag = "springMassDamper" + smd.massInit = 20.0 # [kg] + smd.k = 100.0 # [N/m] + smd.c = 0.0 # [N s/m] undamped + smd.rhoInit = 0.5 # [m] + smd.rhoDotInit = 0.0 # [m/s] + smd.pHat_B = [[1.0], [0.0], [0.0]] # Translation axis (x) + smd.r_PB_B = [[1.0], [0.0], [0.0]] # [m] + sim.AddModelToTask(task_name, smd) + + # Attach the particle to the prescribed motion platform + prescribed_platform.addStateEffector(smd) + + # Add Earth gravity to the simulation + earth_gravity = gravityEffector.GravBodyData() + earth_gravity.planetName = "earth_planet_data" + earth_gravity.mu = 0.3986004415E+15 # meters + earth_gravity.isCentralBody = True + sc_object.gravField.gravBodies = spacecraft.GravBodyVector([earth_gravity]) + + # Set up data logging + conservation_data_log = sc_object.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + prescribed_rotation_data_log = prescribed_platform.prescribedRotationOutMsg.recorder() + sim.AddModelToTask(task_name, conservation_data_log) + sim.AddModelToTask(task_name, prescribed_rotation_data_log) + + # Run the simulation + sim.InitializeSimulation() + sim.ConfigureStopTime(macros.sec2nano(sim_time)) + sim.ExecuteSimulation() + + # Extract the logged variables + orb_energy = conservation_data_log.totOrbEnergy + orb_ang_mom_n = conservation_data_log.totOrbAngMomPntN_N + rot_ang_mom_n = conservation_data_log.totRotAngMomPntC_N + rot_energy = conservation_data_log.totRotEnergy + timespan = prescribed_rotation_data_log.times() * macros.NANO2SEC # [s] + + # Plot results + plot_conservation(timespan, orb_ang_mom_n, orb_energy, rot_ang_mom_n, rot_energy) + + if show_plots: + plt.show() + plt.close("all") + + # Test check + unit_test_check_verification(orb_ang_mom_n, orb_energy, rot_ang_mom_n) + + def create_spacecraft_hub(): mass_hub = 800 # [kg] length_hub = 1.0 # [m] @@ -788,3 +906,4 @@ def unit_test_check_verification(orb_ang_mom_n, orb_energy, rot_ang_mom_n): test_prescribed_branching_spinning_body_one_dof(True) test_prescribed_branching_spinning_body_two_dof(True) test_prescribed_branching_linear_translation_one_dof(True) + test_prescribed_branching_linear_spring_mass_damper(True)