Skip to content

Commit 4429ee3

Browse files
andrewmorellandrewmorell
committed
add branching capabilities to constraintDynamicEffector
1 parent 4d19378 commit 4429ee3

File tree

3 files changed

+199
-41
lines changed

3 files changed

+199
-41
lines changed

‎src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -41,19 +41,19 @@ class DynamicEffector {
4141
Eigen::Vector3d torqueExternalPntB_B = Eigen::Vector3d::Zero(); //!< [Nm] External torque applied by this effector
4242

4343
/** setter for `stateNameOfPosition` property */
44-
void setStateNameOfPosition(std::string value);
44+
virtual void setStateNameOfPosition(std::string value);
4545
/** getter for `stateNameOfPosition` property */
4646
const std::string getStateNameOfPosition() const {return this->stateNameOfPosition; }
4747
/** setter for `stateNameOfVelocity` property */
48-
void setStateNameOfVelocity(std::string value);
48+
virtual void setStateNameOfVelocity(std::string value);
4949
/** getter for `stateNameOfVelocity` property */
5050
const std::string getStateNameOfVelocity() const { return this->stateNameOfVelocity; }
5151
/** setter for `stateNameOfSigma` property */
52-
void setStateNameOfSigma(std::string value);
52+
virtual void setStateNameOfSigma(std::string value);
5353
/** getter for `stateNameOfSigma` property */
5454
const std::string getStateNameOfSigma() const { return this->stateNameOfSigma; }
5555
/** setter for `stateNameOfOmega` property */
56-
void setStateNameOfOmega(std::string value);
56+
virtual void setStateNameOfOmega(std::string value);
5757
/** getter for `stateNameOfOmega` property */
5858
const std::string getStateNameOfOmega() const { return this->stateNameOfOmega; }
5959
/** setter for `propName_m_SC` property */
@@ -85,19 +85,19 @@ class DynamicEffector {
8585
/** getter for `propName_centerOfMassDotSC` property */
8686
const std::string getPropName_centerOfMassDotSC() const { return this->propName_centerOfMassDotSC; }
8787
/** setter for `propName_inertialPosition` property */
88-
void setPropName_inertialPosition(std::string value);
88+
virtual void setPropName_inertialPosition(std::string value);
8989
/** getter for `propName_inertialPosition` property */
9090
const std::string getPropName_inertialPosition() const { return this->propName_inertialPosition; }
9191
/** setter for `propName_inertialVelocity` property */
92-
void setPropName_inertialVelocity(std::string value);
92+
virtual void setPropName_inertialVelocity(std::string value);
9393
/** getter for `propName_inertialVelocity` property */
9494
const std::string getPropName_inertialVelocity() const { return this->propName_inertialVelocity; }
9595
/** setter for `propName_inertialAttitude` property */
96-
void setPropName_inertialAttitude(std::string value);
96+
virtual void setPropName_inertialAttitude(std::string value);
9797
/** getter for `propName_inertialAttitude` property */
9898
const std::string getPropName_inertialAttitude() const { return this->propName_inertialAttitude; }
9999
/** setter for `propName_inertialAngVelocity` property */
100-
void setPropName_inertialAngVelocity(std::string value);
100+
virtual void setPropName_inertialAngVelocity(std::string value);
101101
/** getter for `propName_inertialAngVelocity` property */
102102
const std::string getPropName_inertialAngVelocity() const { return this->propName_inertialAngVelocity; }
103103
/** setter for `propName_vehicleGravity` property */

‎src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp‎

Lines changed: 149 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -35,9 +35,7 @@ ConstraintDynamicEffector::~ConstraintDynamicEffector()
3535

3636
}
3737

38-
/*! This method is used to reset the module.
39-
40-
*/
38+
/*! This method is used to reset the module */
4139
void ConstraintDynamicEffector::Reset(uint64_t CurrentSimNanos)
4240
{
4341
// check if any individual gains are not specified
@@ -123,8 +121,73 @@ void ConstraintDynamicEffector::setC_a(double c_a) {
123121
}
124122
}
125123

126-
/*! This method allows the user to set the cut-off frequency of the low pass filter which is then used to calculate the coefficients for numerical low pass filtering based on a second-order low pass filter design.
124+
void ConstraintDynamicEffector::setStateNameOfPosition(std::string value) {
125+
if (!value.empty()) {
126+
this->stateNameOfPosition.push_back(value);
127+
} else {
128+
bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: stateNameOfPosition variable must be a non-empty string");
129+
}
130+
}
131+
132+
void ConstraintDynamicEffector::setStateNameOfVelocity(std::string value) {
133+
if (!value.empty()) {
134+
this->stateNameOfVelocity.push_back(value);
135+
} else {
136+
bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: stateNameOfVelocity variable must be a non-empty string");
137+
}
138+
}
139+
140+
void ConstraintDynamicEffector::setStateNameOfSigma(std::string value) {
141+
if (!value.empty()) {
142+
this->stateNameOfSigma.push_back(value);
143+
} else {
144+
bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: stateNameOfSigma variable must be a non-empty string");
145+
}
146+
}
147+
148+
void ConstraintDynamicEffector::setStateNameOfOmega(std::string value) {
149+
if (!value.empty()) {
150+
this->stateNameOfOmega.push_back(value);
151+
} else {
152+
bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: stateNameOfOmega variable must be a non-empty string");
153+
}
154+
}
155+
156+
void ConstraintDynamicEffector::setPropName_inertialPosition(std::string value) {
157+
if (!value.empty()) {
158+
this->propName_inertialPosition.push_back(value);
159+
} else {
160+
bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: propName_inertialPosition variable must be a non-empty string");
161+
}
162+
}
163+
164+
void ConstraintDynamicEffector::setPropName_inertialVelocity(std::string value) {
165+
if (!value.empty()) {
166+
this->propName_inertialVelocity.push_back(value);
167+
} else {
168+
bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: propName_inertialVelocity variable must be a non-empty string");
169+
}
170+
}
171+
172+
void ConstraintDynamicEffector::setPropName_inertialAttitude(std::string value) {
173+
if (!value.empty()) {
174+
this->propName_inertialAttitude.push_back(value);
175+
} else {
176+
bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: propName_inertialAttitude variable must be a non-empty string");
177+
}
178+
}
179+
180+
void ConstraintDynamicEffector::setPropName_inertialAngVelocity(std::string value) {
181+
if (!value.empty()) {
182+
this->propName_inertialAngVelocity.push_back(value);
183+
} else {
184+
bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: propName_inertialAngVelocity variable must be a non-empty string");
185+
}
186+
}
127187

188+
/*! This method allows the user to set the cut-off frequency of the low pass filter which is then
189+
used to calculate the coefficients for numerical low pass filtering based on a second-order low
190+
pass filter design.
128191
@param wc The cut-off frequency of the low pass filter.
129192
@param h The constant digital time step.
130193
@param k The damping coefficient
@@ -144,9 +207,7 @@ void ConstraintDynamicEffector::setFilter_Data(double wc, double h, double k){
144207
}
145208
}
146209

147-
/*! This method allows the user to set the status of the constraint dynamic effector
148-
149-
*/
210+
/*! This method allows the user to set the status of the constraint dynamic effector */
150211
void ConstraintDynamicEffector::readInputMessage(){
151212
if(this->effectorStatusInMsg.isLinked()){
152213
DeviceStatusMsgPayload statusMsg;
@@ -159,25 +220,68 @@ void ConstraintDynamicEffector::readInputMessage(){
159220
}
160221

161222
/*! This method allows the constraint effector to have access to the parent states
162-
163223
@param states The states to link
164224
*/
165225
void ConstraintDynamicEffector::linkInStates(DynParamManager& states)
166226
{
167227
if (this->scInitCounter > 1) {
168-
bskLogger.bskLog(BSK_ERROR, "constraintDynamicEffector: tried to attach more than 2 spacecraft");
228+
bskLogger.bskLog(BSK_ERROR, "constraintDynamicEffector: tried to attach more than 2 parents");
169229
}
170230

171-
this->hubSigma.push_back(states.getStateObject("hubSigma"));
172-
this->hubOmega.push_back(states.getStateObject("hubOmega"));
173-
this->hubPosition.push_back(states.getStateObject("hubPosition"));
174-
this->hubVelocity.push_back(states.getStateObject("hubVelocity"));
231+
this->hubSigma.push_back(states.getStateObject(this->stateNameOfSigma[scInitCounter]));
232+
this->hubOmega.push_back(states.getStateObject(this->stateNameOfOmega[scInitCounter]));
233+
this->hubPosition.push_back(states.getStateObject(this->stateNameOfPosition[scInitCounter]));
234+
this->hubVelocity.push_back(states.getStateObject(this->stateNameOfVelocity[scInitCounter]));
235+
236+
if (this->scInitCounter == 0) {
237+
this->parent1.parentType = "hub";
238+
this->parent1.idx = 0;
239+
this->hubCounter = 1;
240+
}
241+
else if (this->scInitCounter == 1) {
242+
this->parent2.parentType = "hub";
243+
if (this->hubCounter == 0) {
244+
this->parent2.idx = 0;
245+
} else if (this->hubCounter == 1) {
246+
this->parent2.idx = 1;
247+
}
248+
}
175249

176250
this->scInitCounter++;
177251
}
178252

179-
/*! This method computes the forces on torques on each spacecraft body.
253+
/*! This method is used to link properties to the constraint effector.
254+
@return void
255+
@param properties The parameter manager to collect from
256+
*/
257+
void ConstraintDynamicEffector::linkInProperties(DynParamManager& properties){
258+
if (this->scInitCounter > 1) {
259+
bskLogger.bskLog(BSK_ERROR, "constraintDynamicEffector: tried to attach more than 2 parents");
260+
}
261+
262+
this->inertialAttitudeProperty.push_back(properties.getPropertyReference(this->propName_inertialAttitude[scInitCounter]));
263+
this->inertialAngVelocityProperty.push_back(properties.getPropertyReference(this->propName_inertialAngVelocity[scInitCounter]));
264+
this->inertialPositionProperty.push_back(properties.getPropertyReference(this->propName_inertialPosition[scInitCounter]));
265+
this->inertialVelocityProperty.push_back(properties.getPropertyReference(this->propName_inertialVelocity[scInitCounter]));
180266

267+
if (this->scInitCounter == 0) {
268+
this->parent1.parentType = "effector";
269+
this->parent1.idx = 0;
270+
this->effectorCounter = 1;
271+
}
272+
else if (this->scInitCounter == 1) {
273+
this->parent2.parentType = "effector";
274+
if (this->effectorCounter == 0) {
275+
this->parent2.idx = 0;
276+
} else if (this->effectorCounter == 1) {
277+
this->parent2.idx = 1;
278+
}
279+
}
280+
281+
this->scInitCounter++;
282+
}
283+
284+
/*! This method computes the forces on torques on each spacecraft body.
181285
@param integTime Integration time
182286
@param timeStep Current integration time step used
183287
*/
@@ -186,17 +290,38 @@ void ConstraintDynamicEffector::computeForceTorque(double integTime, double time
186290
if (this->scInitCounter == 2) { // only proceed once both spacecraft are added
187291
// alternate assigning the constraint force and torque
188292
if (this->scID == 0) { // compute all forces and torques once, assign to spacecraft 1 and store for spacecraft 2
189-
// - Collect states from both spacecraft
190-
Eigen::Vector3d r_B1N_N = this->hubPosition[0]->getState();
191-
Eigen::Vector3d rDot_B1N_N = this->hubVelocity[0]->getState();
192-
Eigen::Vector3d omega_B1N_B1 = this->hubOmega[0]->getState();
293+
Eigen::Vector3d r_B1N_N;
294+
Eigen::Vector3d rDot_B1N_N;
295+
Eigen::Vector3d omega_B1N_B1;
193296
Eigen::MRPd sigma_B1N;
194-
sigma_B1N = (Eigen::Vector3d)this->hubSigma[0]->getState();
195-
Eigen::Vector3d r_B2N_N = this->hubPosition[1]->getState();
196-
Eigen::Vector3d rDot_B2N_N = this->hubVelocity[1]->getState();
197-
Eigen::Vector3d omega_B2N_B2 = this->hubOmega[1]->getState();
297+
Eigen::Vector3d r_B2N_N;
298+
Eigen::Vector3d rDot_B2N_N;
299+
Eigen::Vector3d omega_B2N_B2;
198300
Eigen::MRPd sigma_B2N;
199-
sigma_B2N = (Eigen::Vector3d)this->hubSigma[1]->getState();
301+
if (this->parent1.parentType == "hub") {
302+
// - Collect states from parent spacecraft hub
303+
r_B1N_N = this->hubPosition[parent1.idx]->getState();
304+
rDot_B1N_N = this->hubVelocity[parent1.idx]->getState();
305+
omega_B1N_B1 = this->hubOmega[parent1.idx]->getState();
306+
sigma_B1N = (Eigen::Vector3d)this->hubSigma[parent1.idx]->getState();
307+
} else if (this->parent2.parentType == "effector") {
308+
// - Collect properties from parent effector
309+
r_B1N_N = *this->inertialPositionProperty[parent1.idx];
310+
rDot_B1N_N = *this->inertialVelocityProperty[parent1.idx];
311+
omega_B1N_B1 = *this->inertialAngVelocityProperty[parent1.idx];
312+
sigma_B1N = (Eigen::Vector3d)*this->inertialAttitudeProperty[parent1.idx];
313+
}
314+
if (this->parent2.parentType == "hub") {
315+
r_B2N_N = this->hubPosition[parent2.idx]->getState();
316+
rDot_B2N_N = this->hubVelocity[parent2.idx]->getState();
317+
omega_B2N_B2 = this->hubOmega[parent2.idx]->getState();
318+
sigma_B2N = (Eigen::Vector3d)this->hubSigma[parent2.idx]->getState();
319+
} else if (this->parent2.parentType == "effector") {
320+
r_B2N_N = *this->inertialPositionProperty[parent2.idx];
321+
rDot_B2N_N = *this->inertialVelocityProperty[parent2.idx];
322+
omega_B2N_B2 = *this->inertialAngVelocityProperty[parent2.idx];
323+
sigma_B2N = (Eigen::Vector3d)*this->inertialAttitudeProperty[parent2.idx];
324+
}
200325

201326
// computing direction constraint psi in the N frame
202327
Eigen::Matrix3d dcm_B1N = (sigma_B1N.toRotationMatrix()).transpose();
@@ -252,9 +377,8 @@ void ConstraintDynamicEffector::computeForceTorque(double integTime, double time
252377
}
253378
}
254379

255-
/*! This method takes the computed constraint force and torque states and outputs them to the m
380+
/*! This method takes the computed constraint force and torque states and outputs them to the
256381
messaging system.
257-
258382
@param CurrentClock The current simulation time (used for time stamping)
259383
*/
260384
void ConstraintDynamicEffector::writeOutputStateMessage(uint64_t CurrentClock)
@@ -272,7 +396,6 @@ void ConstraintDynamicEffector::writeOutputStateMessage(uint64_t CurrentClock)
272396
}
273397

274398
/*! Update state method
275-
276399
@param CurrentSimNanos The current simulation time
277400
*/
278401
void ConstraintDynamicEffector::UpdateState(uint64_t CurrentSimNanos)
@@ -286,7 +409,6 @@ void ConstraintDynamicEffector::UpdateState(uint64_t CurrentSimNanos)
286409
}
287410

288411
/*! Filtering method to calculate filtered Constraint Force
289-
290412
@param CurrentClock The current simulation time (used for time stamping)
291413
*/
292414
void ConstraintDynamicEffector::computeFilteredForce(uint64_t CurrentClock)
@@ -305,7 +427,6 @@ void ConstraintDynamicEffector::computeFilteredForce(uint64_t CurrentClock)
305427
}
306428

307429
/*! Filtering method to calculate filtered Constraint Torque
308-
309430
@param CurrentClock The current simulation time (used for time stamping)
310431
*/
311432
void ConstraintDynamicEffector::computeFilteredTorque(uint64_t CurrentClock)

‎src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h‎

Lines changed: 42 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -38,10 +38,11 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector {
3838
public:
3939
ConstraintDynamicEffector();
4040
~ConstraintDynamicEffector();
41-
void Reset(uint64_t CurrentSimNanos);
42-
void linkInStates(DynParamManager& states);
43-
void computeForceTorque(double integTime, double timeStep);
44-
void UpdateState(uint64_t CurrentSimNanos);
41+
void Reset(uint64_t CurrentSimNanos) override;
42+
void linkInStates(DynParamManager& states) override;
43+
void linkInProperties(DynParamManager& properties) override;
44+
void computeForceTorque(double integTime, double timeStep) override;
45+
void UpdateState(uint64_t CurrentSimNanos) override;
4546
void writeOutputStateMessage(uint64_t CurrentClock);
4647
void computeFilteredForce(uint64_t CurrentClock);
4748
void computeFilteredTorque(uint64_t CurrentClock);
@@ -67,6 +68,22 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector {
6768
void setC_a(double c_a);
6869
/** setter for `a,b,s,c,d,e` coefficients of low pass filter */
6970
void setFilter_Data(double wc,double h, double k);
71+
/** setter for `stateNameOfPosition` property */
72+
void setStateNameOfPosition(std::string value) override;
73+
/** setter for `stateNameOfVelocity` property */
74+
void setStateNameOfVelocity(std::string value) override;
75+
/** setter for `stateNameOfSigma` property */
76+
void setStateNameOfSigma(std::string value) override;
77+
/** setter for `stateNameOfOmega` property */
78+
void setStateNameOfOmega(std::string value) override;
79+
/** setter for `propName_inertialPosition` property */
80+
void setPropName_inertialPosition(std::string value) override;
81+
/** setter for `propName_inertialVelocity` property */
82+
void setPropName_inertialVelocity(std::string value) override;
83+
/** setter for `propName_inertialAttitude` property */
84+
void setPropName_inertialAttitude(std::string value) override;
85+
/** setter for `propName_inertialAngVelocity` property */
86+
void setPropName_inertialAngVelocity(std::string value) override;
7087

7188
/** getter for `r_P2P1_B1Init` initial spacecraft separation */
7289
Eigen::Vector3d getR_P2P1_B1Init() const {return this->r_P2P1_B1Init;};
@@ -95,7 +112,13 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector {
95112

96113
// Counters and flags
97114
int scInitCounter = 0; //!< counter to kill simulation if more than two spacecraft initialized
98-
int scID = 1; //!< 0,1 alternating spacecraft tracker to output appropriate force/torque
115+
int scID = 1; //!< 0,1 alternating spacecraft toggle to output appropriate force/torque
116+
struct {
117+
int idx;
118+
std::string parentType;
119+
} parent1, parent2;
120+
int hubCounter = 0;
121+
int effectorCounter = 0;
99122

100123
// Constraint length and direction
101124
Eigen::Vector3d r_P1B1_B1 = Eigen::Vector3d::Zero(); //!< [m] position vector from spacecraft 1 hub to its connection point P1
@@ -137,11 +160,25 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector {
137160
double T2_filtered_mag_tminus2 = 0.0; //!< Magnitude of filtered constraint torque on s/c 2 at t-2 time step
138161

139162
// Simulation variable pointers
163+
std::vector<std::string> stateNameOfPosition; //!< state engine name of the parent rigid body inertial position vector
164+
std::vector<std::string> stateNameOfVelocity; //!< state engine name of the parent rigid body inertial velocity vector
165+
std::vector<std::string> stateNameOfSigma; //!< state engine name of the parent rigid body inertial attitude
166+
std::vector<std::string> stateNameOfOmega; //!< state engine name of the parent rigid body inertial angular velocity vector
140167
std::vector<StateData*> hubPosition; //!< [m] parent inertial position vector
141168
std::vector<StateData*> hubVelocity; //!< [m/s] parent inertial velocity vector
142169
std::vector<StateData*> hubSigma; //!< parent attitude Modified Rodrigues Parameters (MRPs)
143170
std::vector<StateData*> hubOmega; //!< [rad/s] parent inertial angular velocity vector
144171

172+
// Parent body inertial properties
173+
std::vector<std::string> propName_inertialPosition; //!< property name of inertialPosition
174+
std::vector<std::string> propName_inertialVelocity; //!< property name of inertialVelocity
175+
std::vector<std::string> propName_inertialAttitude; //!< property name of inertialAttitude
176+
std::vector<std::string> propName_inertialAngVelocity; //!< property name of inertialAngVelocity
177+
std::vector<Eigen::MatrixXd*> inertialPositionProperty; //!< [m] position relative to inertial frame
178+
std::vector<Eigen::MatrixXd*> inertialVelocityProperty; //!< [m/s] velocity relative to inertial frame
179+
std::vector<Eigen::MatrixXd*> inertialAttitudeProperty; //!< attitude relative to inertial frame
180+
std::vector<Eigen::MatrixXd*> inertialAngVelocityProperty; //!< [rad/s] inertial angular velocity relative to inertial frame
181+
145182
// Constraint violations
146183
Eigen::Vector3d psi_N = Eigen::Vector3d::Zero(); //!< [m] direction constraint violation in inertial frame
147184
Eigen::Vector3d psiPrime_N = Eigen::Vector3d::Zero(); //!< [m/s] direction rate constraint violation in inertial frame

0 commit comments

Comments
 (0)