Feature/1596 configure linearspringmassdamper for prescribed motion branching

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.

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 */

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)