Skip to content

Commit a60baed

Browse files
Change the constraint violation to account for desired relative attitude
1 parent b4cfacb commit a60baed

File tree

2 files changed

+3
-3
lines changed

2 files changed

+3
-3
lines changed

src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -222,7 +222,7 @@ void ConstraintDynamicEffector::computeForceTorque(double integTime, double time
222222
this->psiPrime_N = rDot_P2P1_N - omega_B1N_N.cross(r_P2P1_N);
223223

224224
// calculate the attitude constraint violations
225-
this->sigma_B2B1 = eigenC2MRP(dcm_B2N * dcm_B1N.transpose()); // calculate the difference in attitude
225+
this->phi = eigenC2MRP(dcm_B2N * dcm_B1N.transpose() * this->dcm_B2B1Init.transpose()); // calculate the difference in attitude
226226
Eigen::Vector3d omega_B1N_B2 = dcm_B2N * dcm_B1N.transpose() * omega_B1N_B1;
227227
this->omega_B2B1_B2 = omega_B2N_B2 - omega_B1N_B2; // difference in angular rate
228228

@@ -238,7 +238,7 @@ void ConstraintDynamicEffector::computeForceTorque(double integTime, double time
238238

239239
// calculate the constraint torque imparted on each spacecraft from the attitude constraint
240240
Eigen::Matrix3d dcm_B1B2 = dcm_B1N * dcm_B2N.transpose();
241-
Eigen::Vector3d L_B2_att = -this->k_a * eigenMRPd2Vector3d(sigma_B2B1) - this->c_a * 0.25 * sigma_B2B1.Bmat() * omega_B2B1_B2;
241+
Eigen::Vector3d L_B2_att = -this->k_a * eigenMRPd2Vector3d(this->phi) - this->c_a * 0.25 * this->phi.Bmat() * omega_B2B1_B2;
242242
Eigen::Vector3d L_B1_att = - dcm_B1B2 * L_B2_att;
243243
this->T_B2 = L_B2_len + L_B2_att; // store the constraint torque for spacecraft 2
244244

src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -150,7 +150,7 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector {
150150
// Constraint violations
151151
Eigen::Vector3d psi_N = Eigen::Vector3d::Zero(); //!< [m] direction constraint violation in inertial frame
152152
Eigen::Vector3d psiPrime_N = Eigen::Vector3d::Zero(); //!< [m/s] direction rate constraint violation in inertial frame
153-
Eigen::MRPd sigma_B2B1 = Eigen::MRPd::Identity(); //!< attitude constraint violation
153+
Eigen::MRPd phi = Eigen::MRPd::Identity(); //!< attitude constraint violation
154154
Eigen::Vector3d omega_B2B1_B2 = Eigen::Vector3d::Zero(); //!< [rad/s] angular velocity constraint violation in spacecraft 2 body frame
155155

156156
// Force and torque quantities stored to be assigned on the alternating call of computeForceTorque

0 commit comments

Comments
 (0)