Merge pull request #1105 from AVSLab/feature/add-relative-attitude-constraint-effector

Add relative attitude to the constraint effector

Author: joaogvcarneiro

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)

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
 

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