diff --git a/docs/source/Support/bskReleaseNotes.rst b/docs/source/Support/bskReleaseNotes.rst index 164a0daa5d..c03fae8a05 100644 --- a/docs/source/Support/bskReleaseNotes.rst +++ b/docs/source/Support/bskReleaseNotes.rst @@ -36,6 +36,7 @@ Version |release| Users need to use ``ClassicalElements()`` defined in ``orbitalMotion``. - Fixed bug with recording message payload entries that are 2D arrays. This bug was introduced with the faster recording strategy added in version 2.78.0. +- Add a desired relative attitude between spacecraft in :ref:`constraintDynamicEffector`. Version 2.78.0 (August 30, 2025) diff --git a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp index cc62a868df..261556d868 100644 --- a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp +++ b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp @@ -75,6 +75,10 @@ void ConstraintDynamicEffector::setR_P2B2_B2(Eigen::Vector3d r_P2B2_B2) { this->r_P2B2_B2 = r_P2B2_B2; } +void ConstraintDynamicEffector::setSigma_B2B1Init(Eigen::MRPd sigma_B2B1Init) { + this->dcm_B2B1Init = sigma_B2B1Init.toRotationMatrix().transpose(); +} + void ConstraintDynamicEffector::setAlpha(double alpha) { if (alpha > 0.0) this->alpha = alpha; @@ -218,7 +222,7 @@ void ConstraintDynamicEffector::computeForceTorque(double integTime, double time this->psiPrime_N = rDot_P2P1_N - omega_B1N_N.cross(r_P2P1_N); // calculate the attitude constraint violations - this->sigma_B2B1 = eigenC2MRP(dcm_B2N * dcm_B1N.transpose()); // calculate the difference in attitude + this->phi = eigenC2MRP(dcm_B2N * dcm_B1N.transpose() * this->dcm_B2B1Init.transpose()); // calculate the difference in attitude Eigen::Vector3d omega_B1N_B2 = dcm_B2N * dcm_B1N.transpose() * omega_B1N_B1; this->omega_B2B1_B2 = omega_B2N_B2 - omega_B1N_B2; // difference in angular rate @@ -234,7 +238,7 @@ void ConstraintDynamicEffector::computeForceTorque(double integTime, double time // calculate the constraint torque imparted on each spacecraft from the attitude constraint Eigen::Matrix3d dcm_B1B2 = dcm_B1N * dcm_B2N.transpose(); - Eigen::Vector3d L_B2_att = -this->k_a * eigenMRPd2Vector3d(sigma_B2B1) - this->c_a * 0.25 * sigma_B2B1.Bmat() * omega_B2B1_B2; + Eigen::Vector3d L_B2_att = -this->k_a * eigenMRPd2Vector3d(this->phi) - this->c_a * 0.25 * this->phi.Bmat() * omega_B2B1_B2; Eigen::Vector3d L_B1_att = - dcm_B1B2 * L_B2_att; this->T_B2 = L_B2_len + L_B2_att; // store the constraint torque for spacecraft 2 diff --git a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h index 712e24eb68..d33d5ba06c 100644 --- a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h +++ b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h @@ -53,6 +53,8 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector { void setR_P1B1_B1(Eigen::Vector3d r_P1B1_B1); /** setter for `r_P2B2_B2` connection point position on spacecraft 2 */ void setR_P2B2_B2(Eigen::Vector3d r_P2B2_B2); + /** setter for `sigma_B2B1_Init` initial spacecraft relative attitude */ + void setSigma_B2B1Init(Eigen::MRPd sigma_B2B1Init); /** setter for `alpha` gain tuning parameter */ void setAlpha(double alpha); /** setter for `beta` gain tuning parameter */ @@ -100,7 +102,10 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector { // Constraint length and direction Eigen::Vector3d r_P1B1_B1 = Eigen::Vector3d::Zero(); //!< [m] position vector from spacecraft 1 hub to its connection point P1 Eigen::Vector3d r_P2B2_B2 = Eigen::Vector3d::Zero(); //!< [m] position vector from spacecraft 2 hub to its connection point P2 - Eigen::Vector3d r_P2P1_B1Init = Eigen::Vector3d::Zero(); //!< [m] precribed position vector from spacecraft 1 connection point to spacecraft 2 connection point + Eigen::Vector3d r_P2P1_B1Init = Eigen::Vector3d::Zero(); //!< [m] prescribed position vector from spacecraft 1 connection point to spacecraft 2 connection point + + // Constraint attitude + Eigen::Matrix3d dcm_B2B1Init = Eigen::Matrix3d::Identity(); //!< attitude constraint violation // Gains for PD controller double alpha = 0.0; //!< Baumgarte stabilization gain tuning variable @@ -145,7 +150,7 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector { // Constraint violations Eigen::Vector3d psi_N = Eigen::Vector3d::Zero(); //!< [m] direction constraint violation in inertial frame Eigen::Vector3d psiPrime_N = Eigen::Vector3d::Zero(); //!< [m/s] direction rate constraint violation in inertial frame - Eigen::MRPd sigma_B2B1 = Eigen::MRPd::Identity(); //!< attitude constraint violation + Eigen::MRPd phi = Eigen::MRPd::Identity(); //!< attitude constraint violation Eigen::Vector3d omega_B2B1_B2 = Eigen::Vector3d::Zero(); //!< [rad/s] angular velocity constraint violation in spacecraft 2 body frame // Force and torque quantities stored to be assigned on the alternating call of computeForceTorque