From f56795dc25dd1be3eac04d7a0e74f6b20de96b47 Mon Sep 17 00:00:00 2001 From: Andrew Morell Date: Tue, 17 Dec 2024 14:32:18 -0700 Subject: [PATCH 01/13] add method to stateEffector for attaching dynamicEffectors --- .../dynamics/_GeneralModuleFiles/stateEffector.cpp | 6 ++++++ src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h | 2 ++ 2 files changed, 8 insertions(+) diff --git a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp index 2d1de789f7..c39985e295 100755 --- a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp @@ -95,6 +95,12 @@ void StateEffector::writeOutputStateMessages(uint64_t integTimeNanos) return; } +/*! This method can only be called for a state effector with override definition set up to support attached dynamic effectors */ +void StateEffector::addDynamicEffector(DynamicEffector *newDynamicEffector, int segment) +{ + bskLogger.bskLog(BSK_ERROR, "StateEffector: not compatable with dependent effectors"); +} + /*! This method ensures that stateEffectors can be implemented using the multi-spacecraft archticture */ void StateEffector::prependSpacecraftNameToStates() { diff --git a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h index a5d1c6b777..ed974ae935 100755 --- a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h @@ -24,6 +24,7 @@ #include "architecture/utilities/avsEigenMRP.h" #include "dynParamManager.h" #include "architecture/utilities/bskLogging.h" +#include "simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h" /*! back substitution matrix structure*/ @@ -130,6 +131,7 @@ class StateEffector { virtual void calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B); //!< Force and torque on s/c due to stateEffector virtual void writeOutputStateMessages(uint64_t integTimeNanos); //!< Write State Messages after integration virtual void registerStates(DynParamManager& states) = 0; //!< Method for stateEffectors to register states + virtual void addDynamicEffector(DynamicEffector *newDynamicEffector, int segment); //!< Method to attach a dynamic effector virtual void linkInStates(DynParamManager& states) = 0; //!< Method for stateEffectors to get other states virtual void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN)=0; //!< Method for each stateEffector to calculate derivatives virtual void prependSpacecraftNameToStates(); From 49818a00c1a252972edf7b6887b5b87e976d1c7f Mon Sep 17 00:00:00 2001 From: Andrew Morell Date: Tue, 17 Dec 2024 14:34:06 -0700 Subject: [PATCH 02/13] define virtual methods for property exchanging between effectors --- .../dynamics/_GeneralModuleFiles/dynamicEffector.cpp | 6 ++++++ .../dynamics/_GeneralModuleFiles/dynamicEffector.h | 1 + .../dynamics/_GeneralModuleFiles/stateEffector.cpp | 6 ++++++ src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h | 1 + 4 files changed, 14 insertions(+) diff --git a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.cpp b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.cpp index 529fee66c5..7abdf72474 100755 --- a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.cpp @@ -43,6 +43,12 @@ void DynamicEffector::computeStateContribution(double integTime) return; } +/*! This method */ +void DynamicEffector::linkInProperties(DynParamManager& properties) +{ + return; +} + void DynamicEffector::setStateNameOfPosition(std::string value) { // check that value is acceptable diff --git a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h index ac01c0b086..62ce432752 100755 --- a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h @@ -31,6 +31,7 @@ class DynamicEffector { virtual ~DynamicEffector(); //!< Destructor virtual void computeStateContribution(double integTime); virtual void linkInStates(DynParamManager& states) = 0; //!< Method to get access to other states/stateEffectors + virtual void linkInProperties(DynParamManager& properties); //!< Method to get access to other properties/stateEffectors virtual void computeForceTorque(double integTime, double timeStep) = 0; //!< -- Method to computeForce and torque on the body public: diff --git a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp index c39985e295..c6f8a12a19 100755 --- a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp @@ -95,6 +95,12 @@ void StateEffector::writeOutputStateMessages(uint64_t integTimeNanos) return; } +/*! This method allows the effector to register its properties */ +void StateEffector::registerProperties(DynParamManager& states) +{ + return; +} + /*! This method can only be called for a state effector with override definition set up to support attached dynamic effectors */ void StateEffector::addDynamicEffector(DynamicEffector *newDynamicEffector, int segment) { diff --git a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h index ed974ae935..8f6510aec2 100755 --- a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h @@ -131,6 +131,7 @@ class StateEffector { virtual void calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B); //!< Force and torque on s/c due to stateEffector virtual void writeOutputStateMessages(uint64_t integTimeNanos); //!< Write State Messages after integration virtual void registerStates(DynParamManager& states) = 0; //!< Method for stateEffectors to register states + virtual void registerProperties(DynParamManager& states); //!< Method for stateEffectors to register properties virtual void addDynamicEffector(DynamicEffector *newDynamicEffector, int segment); //!< Method to attach a dynamic effector virtual void linkInStates(DynParamManager& states) = 0; //!< Method for stateEffectors to get other states virtual void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN)=0; //!< Method for each stateEffector to calculate derivatives From 74567d9ed1ade5a6fbe82bd2b6a859c196a3c5e6 Mon Sep 17 00:00:00 2001 From: Andrew Morell Date: Tue, 17 Dec 2024 14:39:16 -0700 Subject: [PATCH 03/13] add attitude and angular velocity properties to dynamicEffector class --- .../_GeneralModuleFiles/dynamicEffector.cpp | 20 +++++++++++++++++++ .../_GeneralModuleFiles/dynamicEffector.h | 10 ++++++++++ 2 files changed, 30 insertions(+) diff --git a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.cpp b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.cpp index 7abdf72474..61449c9fb5 100755 --- a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.cpp @@ -179,6 +179,26 @@ void DynamicEffector::setPropName_inertialVelocity(std::string value) } } +void DynamicEffector::setPropName_inertialAttitude(std::string value) +{ + // check that value is acceptable + if (!value.empty()) { + this->propName_inertialAttitude = value; + } else { + bskLogger.bskLog(BSK_ERROR, "DynamicEffector: propName_inertialAttitude variable must be a non-empty string"); + } +} + +void DynamicEffector::setPropName_inertialAngVelocity(std::string value) +{ + // check that value is acceptable + if (!value.empty()) { + this->propName_inertialAngVelocity = value; + } else { + bskLogger.bskLog(BSK_ERROR, "DynamicEffector: propName_inertialAngVelocity variable must be a non-empty string"); + } +} + void DynamicEffector::setPropName_vehicleGravity(std::string value) { // check that value is acceptable diff --git a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h index 62ce432752..fdbc833408 100755 --- a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h @@ -92,6 +92,14 @@ class DynamicEffector { void setPropName_inertialVelocity(std::string value); /** getter for `propName_inertialVelocity` property */ const std::string getPropName_inertialVelocity() const { return this->propName_inertialVelocity; } + /** setter for `propName_inertialAttitude` property */ + void setPropName_inertialAttitude(std::string value); + /** getter for `propName_inertialAttitude` property */ + const std::string getPropName_inertialAttitude() const { return this->propName_inertialAttitude; } + /** setter for `propName_inertialAngVelocity` property */ + void setPropName_inertialAngVelocity(std::string value); + /** getter for `propName_inertialAngVelocity` property */ + const std::string getPropName_inertialAngVelocity() const { return this->propName_inertialAngVelocity; } /** setter for `propName_vehicleGravity` property */ void setPropName_vehicleGravity(std::string value); /** getter for `propName_vehicleGravity` property */ @@ -114,6 +122,8 @@ class DynamicEffector { std::string propName_centerOfMassDotSC = ""; //!< property name of centerOfMassDotSC std::string propName_inertialPosition = ""; //!< property name of inertialPosition std::string propName_inertialVelocity = ""; //!< property name of inertialVelocity + std::string propName_inertialAttitude = ""; //!< property name of inertialAttitude + std::string propName_inertialAngVelocity = ""; //!< property name of inertialAngVelocity std::string propName_vehicleGravity = ""; //!< property name of vehicleGravity }; From cb5245dface53602cd5611f489b329ddbf036481 Mon Sep 17 00:00:00 2001 From: Andrew Morell Date: Tue, 17 Dec 2024 14:47:22 -0700 Subject: [PATCH 04/13] convert SB1DOF properties to MatrixXd to work with property manager --- .../spinningBodyOneDOFStateEffector.cpp | 15 ++++++++------- .../spinningBodyOneDOFStateEffector.h | 12 ++++++------ 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp index 8594e86fb2..498fa7ad8c 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp @@ -89,10 +89,10 @@ void SpinningBodyOneDOFStateEffector::writeOutputStateMessages(uint64_t CurrentC configLogMsg = this->spinningBodyConfigLogOutMsg.zeroMsgPayload; // Logging the S frame is the body frame B of that object - eigenVector3d2CArray(this->r_ScN_N, configLogMsg.r_BN_N); - eigenVector3d2CArray(this->v_ScN_N, configLogMsg.v_BN_N); - eigenVector3d2CArray(this->sigma_SN, configLogMsg.sigma_BN); - eigenVector3d2CArray(this->omega_SN_S, configLogMsg.omega_BN_B); + eigenMatrixXd2CArray(*this->r_ScN_N, configLogMsg.r_BN_N); + eigenMatrixXd2CArray(*this->v_ScN_N, configLogMsg.v_BN_N); + eigenMatrixXd2CArray(*this->sigma_SN, configLogMsg.sigma_BN); + eigenMatrixXd2CArray(*this->omega_SN_S, configLogMsg.omega_BN_B); this->spinningBodyConfigLogOutMsg.write(&configLogMsg, this->moduleID, CurrentClock); } } @@ -308,13 +308,14 @@ void SpinningBodyOneDOFStateEffector::computeSpinningBodyInertialStates() // inertial attitude Eigen::Matrix3d dcm_SN; dcm_SN = (this->dcm_BS).transpose() * this->dcm_BN; - this->sigma_SN = eigenMRPd2Vector3d(eigenC2MRP(dcm_SN)); + *this->sigma_SN = eigenMRPd2Vector3d(eigenC2MRP(dcm_SN)); + *this->omega_SN_S = (this->dcm_BS).transpose() * this->omega_SN_B; // inertial position vector - this->r_ScN_N = (Eigen::Vector3d)*this->inertialPositionProperty + this->dcm_BN.transpose() * this->r_ScB_B; + *this->r_ScN_N = (Eigen::Vector3d)*this->inertialPositionProperty + this->dcm_BN.transpose() * this->r_ScB_B; // inertial velocity vector - this->v_ScN_N = (Eigen::Vector3d)*this->inertialVelocityProperty + this->dcm_BN.transpose() * this->rDot_ScB_B; + *this->v_ScN_N = (Eigen::Vector3d)*this->inertialVelocityProperty + this->dcm_BN.transpose() * this->rDot_ScB_B; } /*! This method is used so that the simulation will ask SB to update messages */ diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h index 7525b54fa4..7f8838c283 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h @@ -108,18 +108,18 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel { Eigen::Matrix3d IPntSc_B; //!< [kg-m^2] inertia of spinning body about point Sc in B frame components // Spinning body properties - Eigen::Vector3d r_ScN_N{0.0, 0.0, 0.0}; //!< [m] position vector of spinning body center of mass Sc relative to the inertial frame origin N - Eigen::Vector3d v_ScN_N{0.0, 0.0, 0.0}; //!< [m/s] inertial velocity vector of Sc relative to inertial frame - Eigen::Vector3d sigma_SN{0.0, 0.0, 0.0}; //!< -- MRP attitude of frame S relative to inertial frame - Eigen::Vector3d omega_SN_S{0.0, 0.0, 0.0}; //!< [rad/s] inertial spinning body frame angular velocity vector + Eigen::MatrixXd* r_ScN_N; //!< [m] position vector of spinning body center of mass Sc relative to the inertial frame origin N + Eigen::MatrixXd* v_ScN_N; //!< [m/s] inertial velocity vector of Sc relative to inertial frame + Eigen::MatrixXd* sigma_SN; //!< -- MRP attitude of frame S relative to inertial frame + Eigen::MatrixXd* omega_SN_S; //!< [rad/s] inertial spinning body frame angular velocity vector // States double theta = 0.0; //!< [rad] spinning body angle double thetaDot = 0.0; //!< [rad/s] spinning body angle rate Eigen::MatrixXd* inertialPositionProperty = nullptr; //!< [m] r_N inertial position relative to system spice zeroBase/refBase Eigen::MatrixXd* inertialVelocityProperty = nullptr; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase - StateData *thetaState = nullptr; //!< -- state manager of theta for spinning body - StateData *thetaDotState = nullptr; //!< -- state manager of thetaDot for spinning body + StateData* thetaState = nullptr; //!< -- state manager of theta for spinning body + StateData* thetaDotState = nullptr; //!< -- state manager of thetaDot for spinning body }; From 7de94054c7d97a6a03109a33d99f89afa5fd56e2 Mon Sep 17 00:00:00 2001 From: Andrew Morell Date: Tue, 17 Dec 2024 14:49:29 -0700 Subject: [PATCH 05/13] add property linking between SB1DOF and attached effectors --- .../spinningBodyOneDOFStateEffector.cpp | 21 +++++++++++++++++++ .../spinningBodyOneDOFStateEffector.h | 14 +++++++++++++ 2 files changed, 35 insertions(+) diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp index 498fa7ad8c..798fceaace 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp @@ -47,6 +47,10 @@ SpinningBodyOneDOFStateEffector::SpinningBodyOneDOFStateEffector() this->nameOfThetaState = "spinningBodyTheta" + std::to_string(SpinningBodyOneDOFStateEffector::effectorID); this->nameOfThetaDotState = "spinningBodyThetaDot" + std::to_string(SpinningBodyOneDOFStateEffector::effectorID); + this->nameOfInertialPositionProperty = "spinningBodyInertialPosition" + std::to_string(SpinningBodyOneDOFStateEffector::effectorID); + this->nameOfInertialVelocityProperty = "spinningBodyInertialVelocity" + std::to_string(SpinningBodyOneDOFStateEffector::effectorID); + this->nameOfInertialAttitudeProperty = "spinningBodyInertialAttitude" + std::to_string(SpinningBodyOneDOFStateEffector::effectorID); + this->nameOfInertialAngVelocityProperty = "spinningBodyInertialAngVelocity" + std::to_string(SpinningBodyOneDOFStateEffector::effectorID); SpinningBodyOneDOFStateEffector::effectorID++; } @@ -126,6 +130,23 @@ void SpinningBodyOneDOFStateEffector::registerStates(DynParamManager& states) Eigen::MatrixXd thetaDotInitMatrix(1,1); thetaDotInitMatrix(0,0) = this->thetaDotInit; this->thetaDotState->setState(thetaDotInitMatrix); + + registerProperties(states); +} + +void SpinningBodyOneDOFStateEffector::registerProperties(DynParamManager& states) +{ + Eigen::Vector3d stateInit = Eigen::Vector3d::Zero(); + this->r_ScN_N = states.createProperty(this->nameOfInertialPositionProperty, stateInit); + this->v_ScN_N = states.createProperty(this->nameOfInertialVelocityProperty, stateInit); + this->sigma_SN = states.createProperty(this->nameOfInertialAttitudeProperty, stateInit); + this->omega_SN_S = states.createProperty(this->nameOfInertialAngVelocityProperty, stateInit); + + std::vector::iterator dynIt; + for(dynIt = this->dynEffectors.begin(); dynIt != this->dynEffectors.end(); dynIt++) + { + (*dynIt)->linkInProperties(states); + } } /*! This method allows the SB state effector to provide its contributions to the mass props and mass prop rates of the diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h index 7f8838c283..ef4a45cfda 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h @@ -45,6 +45,10 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel { double thetaDotInit = 0.0; //!< [rad/s] initial spinning body angle rate std::string nameOfThetaState; //!< -- identifier for the theta state data container std::string nameOfThetaDotState; //!< -- identifier for the thetaDot state data container + std::string nameOfInertialPositionProperty; //!< -- identifier for the inertial position property + std::string nameOfInertialVelocityProperty; //!< -- identifier for the inertial velocity property + std::string nameOfInertialAttitudeProperty; //!< -- identifier for the inertial attitude property + std::string nameOfInertialAngVelocityProperty; //!< -- identifier for the inertial angular velocity property Eigen::Vector3d r_SB_B{0.0, 0.0, 0.0}; //!< [m] vector pointing from body frame B origin to spinning frame S origin in B frame components Eigen::Vector3d r_ScS_S{0.0, 0.0, 0.0}; //!< [m] vector pointing from spinning frame S origin to point Sc (center of mass of the spinner) in S frame components Eigen::Vector3d sHat_S{1.0, 0.0, 0.0}; //!< -- spinning axis in S frame components. @@ -63,6 +67,7 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel { void UpdateState(uint64_t CurrentSimNanos) override; //!< -- Method for updating information void registerStates(DynParamManager& statesIn) override; //!< -- Method for registering the SB states void linkInStates(DynParamManager& states) override; //!< -- Method for getting access to other states + void registerProperties(DynParamManager& states) override; //!< -- Method for registering the SB inertial properties void updateContributions(double integTime, BackSubMatrices& backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) override; //!< -- Method for back-substitution contributions @@ -81,6 +86,15 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel { double thetaRef = 0.0; //!< [rad] spinning body reference angle double thetaDotRef = 0.0; //!< [rad] spinning body reference angle rate + template + /** Assign the state engine parameter names */ + void assignStateParamNames(Type effector) { + effector->setPropName_inertialPosition(this->nameOfInertialPositionProperty); + effector->setPropName_inertialVelocity(this->nameOfInertialVelocityProperty); + effector->setPropName_inertialAttitude(this->nameOfInertialAttitudeProperty); + effector->setPropName_inertialAngVelocity(this->nameOfInertialAngVelocityProperty); + }; + // Terms needed for back substitution Eigen::Vector3d aTheta{0.0, 0.0, 0.0}; //!< -- rDDot_BN term for back substitution Eigen::Vector3d bTheta{0.0, 0.0, 0.0}; //!< -- omegaDot_BN term for back substitution From 1a160a8aaf460eb19b707b6d94cc67526f7aabb9 Mon Sep 17 00:00:00 2001 From: Andrew Morell Date: Tue, 17 Dec 2024 14:50:55 -0700 Subject: [PATCH 06/13] define the SB1DOF method for attaching dynamic effectors --- .../spinningBodyOneDOFStateEffector.cpp | 11 +++++++++++ .../spinningBodyOneDOFStateEffector.h | 3 +++ 2 files changed, 14 insertions(+) diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp index 798fceaace..34ab53e7bd 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp @@ -134,6 +134,17 @@ void SpinningBodyOneDOFStateEffector::registerStates(DynParamManager& states) registerProperties(states); } +void SpinningBodyOneDOFStateEffector::addDynamicEffector(DynamicEffector *newDynamicEffector, int segment = 0) +{ + if (segment != 0 && segment != 1) { + bskLogger.bskLog(BSK_ERROR, "Specifying attachment to a non-existent spinning bodies linkage."); + } + + this->assignStateParamNames(newDynamicEffector); + + this->dynEffectors.push_back(newDynamicEffector); +} + void SpinningBodyOneDOFStateEffector::registerProperties(DynParamManager& states) { Eigen::Vector3d stateInit = Eigen::Vector3d::Zero(); diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h index ef4a45cfda..f1a816fbf2 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h @@ -22,6 +22,7 @@ #include #include "simulation/dynamics/_GeneralModuleFiles/stateEffector.h" +#include "simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h" #include "simulation/dynamics/_GeneralModuleFiles/stateData.h" #include "architecture/_GeneralModuleFiles/sys_model.h" #include "architecture/utilities/avsEigenMRP.h" @@ -59,6 +60,7 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel { ReadFunctor motorTorqueInMsg; //!< -- (optional) motor torque input message ReadFunctor motorLockInMsg; //!< -- (optional) motor lock flag input message ReadFunctor spinningBodyRefInMsg; //!< -- (optional) spinning body reference input message name + std::vector dynEffectors; //!< Vector of dynamic effectors attached SpinningBodyOneDOFStateEffector(); //!< -- Contructor ~SpinningBodyOneDOFStateEffector() override; //!< -- Destructor @@ -67,6 +69,7 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel { void UpdateState(uint64_t CurrentSimNanos) override; //!< -- Method for updating information void registerStates(DynParamManager& statesIn) override; //!< -- Method for registering the SB states void linkInStates(DynParamManager& states) override; //!< -- Method for getting access to other states + void addDynamicEffector(DynamicEffector *newDynamicEffector, int segment) override; //!< -- Method for adding attached dynamic effector void registerProperties(DynParamManager& states) override; //!< -- Method for registering the SB inertial properties void updateContributions(double integTime, BackSubMatrices& backSubContr, Eigen::Vector3d sigma_BN, From 2923758e529eabab979d8c3960dedd1ae0f3063c Mon Sep 17 00:00:00 2001 From: Andrew Morell Date: Tue, 17 Dec 2024 14:51:56 -0700 Subject: [PATCH 07/13] loop over SB1DOF attached effectors to update contributions --- .../spinningBodyOneDOFStateEffector.cpp | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp index 34ab53e7bd..2ad0774199 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp @@ -241,6 +241,18 @@ void SpinningBodyOneDOFStateEffector::updateContributions(double integTime, // Define auxiliary variable mTheta this->mTheta = this->sHat_B.transpose() * IPntS_B * this->sHat_B; + // Loop through to collect forces and torques from any connected dynamic effectors + Eigen::Vector3d attBodyForce_S = Eigen::Vector3d::Zero(); + Eigen::Vector3d attBodyTorquePntS_S = Eigen::Vector3d::Zero(); + std::vector::iterator dynIt; + for(dynIt = this->dynEffectors.begin(); dynIt != this->dynEffectors.end(); dynIt++) + { + // - Compute the force and torque contributions from the dynamicEffectors + (*dynIt)->computeForceTorque(integTime, double(0.0)); + attBodyForce_S += (*dynIt)->forceExternal_B; + attBodyTorquePntS_S += (*dynIt)->torqueExternalPntB_B; + } + // Lock the axis if the flag is set to 1 if (this->lockFlag == 1) { @@ -262,7 +274,7 @@ void SpinningBodyOneDOFStateEffector::updateContributions(double integTime, Eigen::Vector3d gravityTorquePntS_B = rTilde_ScS_B * this->mass * g_B; this->cTheta = (this->u - this->k * (this->theta - this->thetaRef) - this->c * (this->thetaDot - this->thetaDotRef) + this->sHat_B.dot(gravityTorquePntS_B - omegaTilde_SN_B * IPntS_B * this->omega_SN_B - - IPntS_B * this->omegaTilde_BN_B * this->omega_SB_B + - IPntS_B * this->omegaTilde_BN_B * this->omega_SB_B + this->dcm_BS * attBodyTorquePntS_S - this->mass * rTilde_ScS_B * this->omegaTilde_BN_B * rDot_SB_B)) / this->mTheta; } @@ -271,7 +283,7 @@ void SpinningBodyOneDOFStateEffector::updateContributions(double integTime, backSubContr.matrixA = -this->mass * rTilde_ScS_B * this->sHat_B * this->aTheta.transpose(); backSubContr.matrixB = -this->mass * rTilde_ScS_B * this->sHat_B * this->bTheta.transpose(); backSubContr.vecTrans = -this->mass * this->omegaTilde_SB_B * this->rPrime_ScS_B - + this->mass * rTilde_ScS_B * this->sHat_B * this->cTheta; + + this->mass * rTilde_ScS_B * this->sHat_B * this->cTheta + this->dcm_BS * attBodyForce_S; // Rotation contributions backSubContr.matrixC = (this->IPntSc_B - this->mass * this->rTilde_ScB_B * rTilde_ScS_B) @@ -281,7 +293,8 @@ void SpinningBodyOneDOFStateEffector::updateContributions(double integTime, backSubContr.vecRot = -omegaTilde_SN_B * this->IPntSc_B * this->omega_SB_B - this->mass * this->omegaTilde_BN_B * this->rTilde_ScB_B * this->rPrime_ScB_B - this->mass * this->rTilde_ScB_B * this->omegaTilde_SB_B * this->rPrime_ScS_B - - (this->IPntSc_B - this->mass * this->rTilde_ScB_B * rTilde_ScS_B) * this->sHat_B * this->cTheta; + - (this->IPntSc_B - this->mass * this->rTilde_ScB_B * rTilde_ScS_B) * this->sHat_B * this->cTheta + + this->dcm_BS * attBodyTorquePntS_S + eigenTilde(this->r_SB_B) * (this->dcm_BS * attBodyForce_S); } /*! This method is used to find the derivatives for the SB stateEffector: thetaDDot and the kinematic derivative */ From e782798c82c0c653e9e88aa441d7c01e44c1c234 Mon Sep 17 00:00:00 2001 From: Andrew Morell Date: Tue, 17 Dec 2024 14:52:51 -0700 Subject: [PATCH 08/13] perform same adaptations to SB2DOF effector --- .../spinningBodyTwoDOFStateEffector.cpp | 104 ++++++++++++++---- .../spinningBodyTwoDOFStateEffector.h | 69 ++++++++---- 2 files changed, 133 insertions(+), 40 deletions(-) diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp index 3fa2419be3..8b11ecb907 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp @@ -56,6 +56,14 @@ SpinningBodyTwoDOFStateEffector::SpinningBodyTwoDOFStateEffector() this->nameOfTheta1DotState = "spinningBodyTheta1Dot" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); this->nameOfTheta2State = "spinningBodyTheta2" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); this->nameOfTheta2DotState = "spinningBodyTheta2Dot" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialPositionProperty1 = "spinningBodyInertialPosition1" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialVelocityProperty1 = "spinningBodyInertialVelocity1" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialAttitudeProperty1 = "spinningBodyInertialAttitude1" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialAngVelocityProperty1 = "spinningBodyInertialAngVelocity1" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialPositionProperty2 = "spinningBodyInertialPosition2" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialVelocityProperty2 = "spinningBodyInertialVelocity2" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialAttitudeProperty2 = "spinningBodyInertialAttitude2" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); + this->nameOfInertialAngVelocityProperty2 = "spinningBodyInertialAngVelocity2" + std::to_string(SpinningBodyTwoDOFStateEffector::effectorID); SpinningBodyTwoDOFStateEffector::effectorID++; } @@ -111,10 +119,10 @@ void SpinningBodyTwoDOFStateEffector::writeOutputStateMessages(uint64_t CurrentC configLogMsg = this->spinningBodyConfigLogOutMsgs[0]->zeroMsgPayload; // Logging the S frame is the body frame B of that object - eigenVector3d2CArray(this->r_Sc1N_N, configLogMsg.r_BN_N); - eigenVector3d2CArray(this->v_Sc1N_N, configLogMsg.v_BN_N); - eigenVector3d2CArray(this->sigma_S1N, configLogMsg.sigma_BN); - eigenVector3d2CArray(this->omega_S1N_S1, configLogMsg.omega_BN_B); + eigenMatrixXd2CArray((*this->r_Sc1N_N), configLogMsg.r_BN_N); + eigenMatrixXd2CArray((*this->v_Sc1N_N), configLogMsg.v_BN_N); + eigenMatrixXd2CArray((*this->sigma_S1N), configLogMsg.sigma_BN); + eigenMatrixXd2CArray((*this->omega_S1N_S1), configLogMsg.omega_BN_B); this->spinningBodyConfigLogOutMsgs[0]->write(&configLogMsg, this->moduleID, CurrentClock); } @@ -123,10 +131,10 @@ void SpinningBodyTwoDOFStateEffector::writeOutputStateMessages(uint64_t CurrentC configLogMsg = this->spinningBodyConfigLogOutMsgs[1]->zeroMsgPayload; // Logging the S frame is the body frame B of that object - eigenVector3d2CArray(this->r_Sc2N_N, configLogMsg.r_BN_N); - eigenVector3d2CArray(this->v_Sc2N_N, configLogMsg.v_BN_N); - eigenVector3d2CArray(this->sigma_S2N, configLogMsg.sigma_BN); - eigenVector3d2CArray(this->omega_S2N_S2, configLogMsg.omega_BN_B); + eigenMatrixXd2CArray(*this->r_Sc2N_N, configLogMsg.r_BN_N); + eigenMatrixXd2CArray(*this->v_Sc2N_N, configLogMsg.v_BN_N); + eigenMatrixXd2CArray(*this->sigma_S2N, configLogMsg.sigma_BN); + eigenMatrixXd2CArray(*this->omega_S2N_S2, configLogMsg.omega_BN_B); this->spinningBodyConfigLogOutMsgs[1]->write(&configLogMsg, this->moduleID, CurrentClock); } } @@ -167,6 +175,35 @@ void SpinningBodyTwoDOFStateEffector::registerStates(DynParamManager& states) thetaDotInitMatrix(1,0) = this->theta2DotInit; this->theta1DotState->setState(thetaDotInitMatrix.row(0)); this->theta2DotState->setState(thetaDotInitMatrix.row(1)); + + registerProperties(states); +} + +void SpinningBodyTwoDOFStateEffector::addDynamicEffector(DynamicEffector *newDynamicEffector, int segment) +{ + this->assignStateParamNames(newDynamicEffector, segment); + + this->dynEffectors.push_back(newDynamicEffector); +} + +void SpinningBodyTwoDOFStateEffector::registerProperties(DynParamManager& states) +{ + Eigen::Vector3d stateInit = Eigen::Vector3d::Zero(); + this->r_Sc1N_N = states.createProperty(this->nameOfInertialPositionProperty1, stateInit); + this->v_Sc1N_N = states.createProperty(this->nameOfInertialVelocityProperty1, stateInit); + this->sigma_S1N = states.createProperty(this->nameOfInertialAttitudeProperty1, stateInit); + this->omega_S1N_S1 = states.createProperty(this->nameOfInertialAngVelocityProperty1, stateInit); + + this->r_Sc2N_N = states.createProperty(this->nameOfInertialPositionProperty2, stateInit); + this->v_Sc2N_N = states.createProperty(this->nameOfInertialVelocityProperty2, stateInit); + this->sigma_S2N = states.createProperty(this->nameOfInertialAttitudeProperty2, stateInit); + this->omega_S2N_S2 = states.createProperty(this->nameOfInertialAngVelocityProperty2, stateInit); + + std::vector::iterator dynIt; + for(dynIt = this->dynEffectors.begin(); dynIt != this->dynEffectors.end(); dynIt++) + { + (*dynIt)->linkInProperties(states); + } } /*! This method allows the SB state effector to provide its contributions to the mass props and mass prop rates of the @@ -271,6 +308,26 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back gLocal_N = g_N; g_B = this->dcm_BN * gLocal_N; + // Loop through to collect forces and torques from any connected dynamic effectors + Eigen::Vector3d attBodyForce_S1 = Eigen::Vector3d::Zero(); + Eigen::Vector3d attBodyTorquePntS1_S1 = Eigen::Vector3d::Zero(); + Eigen::Vector3d attBodyForce_S2 = Eigen::Vector3d::Zero(); + Eigen::Vector3d attBodyTorquePntS2_S2 = Eigen::Vector3d::Zero(); + std::vector::iterator dynIt; + for(dynIt = this->dynEffectors.begin(); dynIt != this->dynEffectors.end(); dynIt++) + { + // - Compute the force and torque contributions from the dynamicEffectors + (*dynIt)->computeForceTorque(integTime, double(0.0)); + if ((*dynIt)->getPropName_inertialPosition() == this->nameOfInertialPositionProperty1) { + attBodyForce_S1 += (*dynIt)->forceExternal_B; + attBodyTorquePntS1_S1 += (*dynIt)->torqueExternalPntB_B; + } + else if ((*dynIt)->getPropName_inertialPosition() == this->nameOfInertialPositionProperty2) { + attBodyForce_S2 += (*dynIt)->forceExternal_B; + attBodyTorquePntS2_S2 += (*dynIt)->torqueExternalPntB_B; + } + } + // Update omega_BN_B this->omega_BN_B = omega_BN_B; this->omegaTilde_BN_B = eigenTilde(this->omega_BN_B); @@ -334,12 +391,15 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back + this->mass1 * (rTilde_Sc1S1_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * rTilde_Sc1S1_B) * this->rPrime_Sc1S1_B + this->mass2 * (rTilde_Sc2S1_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * rTilde_Sc2S1_B) * this->rPrime_Sc2S1_B + this->mass2 * rTilde_Sc2S1_B * omegaTilde_S2S1_B * this->rPrime_Sc2S2_B - + this->mass * rTilde_ScS1_B * this->omegaTilde_BN_B * rDot_S1B_B); + + this->mass * rTilde_ScS1_B * this->omegaTilde_BN_B * rDot_S1B_B + - this->dcm_BS1 * attBodyTorquePntS1_S1 + this->dcm_BS2 * attBodyTorquePntS2_S2 + - eigenTilde(this->r_S2S1_B) * (this->dcm_BS2 * attBodyForce_S2)); CThetaStar(1, 0) = this->u2 - this->k2 * (this->theta2 - this->theta2Ref) - this->c2 * (this->theta2Dot - this->theta2DotRef) + this->s2Hat_B.transpose() * gravityTorquePntS2_B - this->s2Hat_B.transpose() * ((IPrimeS2PntS2_B + this->omegaTilde_BN_B * IS2PntS2_B) * this->omega_S2N_B + IS2PntS2_B * this->omegaTilde_S1B_B * this->omega_S2S1_B + this->mass2 * rTilde_Sc2S2_B * omegaTilde_S1N_B * this->rPrime_S2S1_B - + this->mass2 * rTilde_Sc2S2_B * this->omegaTilde_BN_B * (rDot_S2S1_B + rDot_S1B_B)); + + this->mass2 * rTilde_Sc2S2_B * this->omegaTilde_BN_B * (rDot_S2S1_B + rDot_S1B_B) + - this->dcm_BS2 * attBodyTorquePntS2_S2); // Check if any of the axis are locked and change dynamics accordingly if (this->lockFlag1 == 1 || this->lockFlag2 == 1) @@ -373,7 +433,8 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back backSubContr.matrixA = - this->mass * rTilde_ScS1_B * this->s1Hat_B * this->ATheta.row(0) - this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->ATheta.row(1); backSubContr.matrixB = - this->mass * rTilde_ScS1_B * this->s1Hat_B * this->BTheta.row(0) - this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->BTheta.row(1); backSubContr.vecTrans = - this->mass * this->omegaTilde_S1B_B * this->rPrime_ScB_B - this->mass2 * (omegaTilde_S2S1_B * this->rPrime_Sc2S2_B - rTilde_Sc2S2_B * this->omegaTilde_S1B_B * this->omega_S2S1_B) - + this->mass * rTilde_ScS1_B * this->s1Hat_B * this->CTheta.row(0) + this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->CTheta.row(1); + + this->mass * rTilde_ScS1_B * this->s1Hat_B * this->CTheta.row(0) + this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->CTheta.row(1) + + this->dcm_BS1 * attBodyForce_S1 + this->dcm_BS2 * attBodyForce_S2; // Rotation contributions backSubContr.matrixC = (this->IS1PntSc1_B + this->IS2PntSc2_B - this->mass1 * this->rTilde_Sc1B_B * rTilde_Sc1S1_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S1_B) * this->s1Hat_B * this->ATheta.row(0) @@ -386,7 +447,10 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back - this->mass2 * (this->rTilde_Sc2B_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * this->rTilde_Sc2B_B) * this->rPrime_Sc2B_B - this->mass2 * this->rTilde_Sc2B_B * omegaTilde_S2S1_B * this->rPrime_Sc2S2_B - (this->IS1PntSc1_B + this->IS2PntSc2_B - this->mass1 * this->rTilde_Sc1B_B * rTilde_Sc1S1_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S1_B) * this->s1Hat_B * this->CTheta.row(0) - - (this->IS2PntSc2_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S2_B) * this->s2Hat_B * this->CTheta.row(1); + - (this->IS2PntSc2_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S2_B) * this->s2Hat_B * this->CTheta.row(1) + + this->dcm_BS1 * attBodyTorquePntS1_S1 + this->dcm_BS2 * attBodyTorquePntS2_S2 + + eigenTilde(this->r_S1B_B) * (this->dcm_BS1 * attBodyForce_S1) + + eigenTilde(this->r_S2S1_B + this->r_S1B_B) * (this->dcm_BS2 * attBodyForce_S2); } /*! This method is used to find the derivatives for the SB stateEffector: thetaDDot and the kinematic derivative */ @@ -439,20 +503,20 @@ void SpinningBodyTwoDOFStateEffector::computeSpinningBodyInertialStates() Eigen::Matrix3d dcm_S2N; dcm_S1N = (this->dcm_BS1).transpose() * this->dcm_BN; dcm_S2N = (this->dcm_BS2).transpose() * this->dcm_BN; - this->sigma_S1N = eigenMRPd2Vector3d(eigenC2MRP(dcm_S1N)); - this->sigma_S2N = eigenMRPd2Vector3d(eigenC2MRP(dcm_S2N)); + *this->sigma_S1N = eigenMRPd2Vector3d(eigenC2MRP(dcm_S1N)); + *this->sigma_S2N = eigenMRPd2Vector3d(eigenC2MRP(dcm_S2N)); // Convert the angular velocity to the corresponding frame - this->omega_S1N_S1 = dcm_BS1.transpose() * this->omega_S1N_B; - this->omega_S2N_S2 = dcm_BS2.transpose() * this->omega_S2N_B; + *this->omega_S1N_S1 = dcm_BS1.transpose() * this->omega_S1N_B; + *this->omega_S2N_S2 = dcm_BS2.transpose() * this->omega_S2N_B; // Compute the inertial position vector - this->r_Sc1N_N = (Eigen::Vector3d)(*this->inertialPositionProperty) + this->dcm_BN.transpose() * this->r_Sc1B_B; - this->r_Sc2N_N = (Eigen::Vector3d)(*this->inertialPositionProperty) + this->dcm_BN.transpose() * this->r_Sc2B_B; + *this->r_Sc1N_N = (Eigen::Vector3d)(*this->inertialPositionProperty) + this->dcm_BN.transpose() * this->r_Sc1B_B; + *this->r_Sc2N_N = (Eigen::Vector3d)(*this->inertialPositionProperty) + this->dcm_BN.transpose() * this->r_Sc2B_B; // Compute the inertial velocity vector - this->v_Sc1N_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) + this->dcm_BN.transpose() * this->rDot_Sc1B_B; - this->v_Sc2N_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) + this->dcm_BN.transpose() * this->rDot_Sc2B_B; + *this->v_Sc1N_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) + this->dcm_BN.transpose() * this->rDot_Sc1B_B; + *this->v_Sc2N_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) + this->dcm_BN.transpose() * this->rDot_Sc2B_B; } /*! This method is used so that the simulation will ask SB to update messages */ diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h index e33e3da85d..7c86787fc2 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h @@ -22,6 +22,7 @@ #include #include "simulation/dynamics/_GeneralModuleFiles/stateEffector.h" +#include "simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h" #include "simulation/dynamics/_GeneralModuleFiles/stateData.h" #include "architecture/_GeneralModuleFiles/sys_model.h" #include "architecture/utilities/avsEigenMRP.h" @@ -51,6 +52,14 @@ class SpinningBodyTwoDOFStateEffector: public StateEffector, public SysModel { std::string nameOfTheta1DotState; //!< -- identifier for the thetaDot1 state data container std::string nameOfTheta2State; //!< -- identifier for the theta2 state data container std::string nameOfTheta2DotState; //!< -- identifier for the thetaDot2 state data container + std::string nameOfInertialPositionProperty1; //!< -- identifier for the lower spinning body inertial position property + std::string nameOfInertialVelocityProperty1; //!< -- identifier for the lower spinning body inertial velocity property + std::string nameOfInertialAttitudeProperty1; //!< -- identifier for the lower spinning body inertial attitude property + std::string nameOfInertialAngVelocityProperty1; //!< -- identifier for the lower spinning body inertial angular velocity property + std::string nameOfInertialPositionProperty2; //!< -- identifier for the upper spinning body inertial position property + std::string nameOfInertialVelocityProperty2; //!< -- identifier for the upper spinning body inertial velocity property + std::string nameOfInertialAttitudeProperty2; //!< -- identifier for the upper spinning body inertial attitude property + std::string nameOfInertialAngVelocityProperty2; //!< -- identifier for the upper spinning body inertial angular velocity property Eigen::Vector3d r_S1B_B{0.0,0.0,0.0}; //!< [m] vector pointing from body frame B origin to lower spinning frame S1 origin in B frame components Eigen::Vector3d r_S2S1_S1{0.0,0.0,0.0}; //!< [m] vector pointing from lower spinning frame S1 origin to upper spinning frame S2 origin in S1 frame components Eigen::Vector3d r_Sc1S1_S1{0.0,0.0,0.0}; //!< [m] vector pointing from lower spinning frame S1 origin to point Sc1 (center of mass of the lower spinner) in S1 frame components @@ -69,29 +78,32 @@ class SpinningBodyTwoDOFStateEffector: public StateEffector, public SysModel { ReadFunctor motorLockInMsg; //!< -- (optional) motor lock input message name std::vector> spinningBodyRefInMsgs {ReadFunctor(), ReadFunctor()}; //!< (optional) vector of spinning body reference input messages + std::vector dynEffectors; //!< Vector of dynamic effectors attached SpinningBodyTwoDOFStateEffector(); //!< -- Contructor ~SpinningBodyTwoDOFStateEffector(); //!< -- Destructor - void Reset(uint64_t CurrentClock); //!< -- Method for reset - void writeOutputStateMessages(uint64_t CurrentClock); //!< -- Method for writing the output messages - void UpdateState(uint64_t CurrentSimNanos); //!< -- Method for updating information - void registerStates(DynParamManager& statesIn); //!< -- Method for registering the SB states - void linkInStates(DynParamManager& states); //!< -- Method for getting access to other states + void Reset(uint64_t CurrentClock) override; //!< -- Method for reset + void writeOutputStateMessages(uint64_t CurrentClock) override; //!< -- Method for writing the output messages + void UpdateState(uint64_t CurrentSimNanos) override; //!< -- Method for updating information + void registerStates(DynParamManager& statesIn) override; //!< -- Method for registering the SB states + void linkInStates(DynParamManager& states) override; //!< -- Method for getting access to other states + void addDynamicEffector(DynamicEffector *newDynamicEffector, int segment) override; //!< -- Method for adding attached dynamic effector + void registerProperties(DynParamManager& states) override; //!< -- Method for registering the SB inertial properties void updateContributions(double integTime, BackSubMatrices& backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, - Eigen::Vector3d g_N); //!< -- Method for back-substitution contributions + Eigen::Vector3d g_N) override; //!< -- Method for back-substitution contributions void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, - Eigen::Vector3d sigma_BN); //!< -- Method for SB to compute its derivatives - void updateEffectorMassProps(double integTime); //!< -- Method for giving the s/c the HRB mass props and prop rates + Eigen::Vector3d sigma_BN) override; //!< -- Method for SB to compute its derivatives + void updateEffectorMassProps(double integTime) override; //!< -- Method for giving the s/c the HRB mass props and prop rates void updateEnergyMomContributions(double integTime, Eigen::Vector3d& rotAngMomPntCContr_B, double& rotEnergyContr, - Eigen::Vector3d omega_BN_B); //!< -- Method for computing energy and momentum for SBs - void prependSpacecraftNameToStates(); //!< Method used for multiple spacecraft + Eigen::Vector3d omega_BN_B) override; //!< -- Method for computing energy and momentum for SBs + void prependSpacecraftNameToStates() override; //!< Method used for multiple spacecraft void computeSpinningBodyInertialStates(); //!< Method for computing the SB's states private: @@ -106,6 +118,23 @@ class SpinningBodyTwoDOFStateEffector: public StateEffector, public SysModel { double theta2DotRef = 0.0; //!< [rad] spinning body reference angle rate double mass = 1.0; //!< [kg] mass of the spinner system + template + /** Assign the state engine parameter names */ + void assignStateParamNames(Type effector, int segment) { + if (segment == 1) { + effector->setPropName_inertialPosition(this->nameOfInertialPositionProperty1); + effector->setPropName_inertialVelocity(this->nameOfInertialVelocityProperty1); + effector->setPropName_inertialAttitude(this->nameOfInertialAttitudeProperty1); + effector->setPropName_inertialAngVelocity(this->nameOfInertialAngVelocityProperty1); + } + else if (segment == 2) { + effector->setPropName_inertialPosition(this->nameOfInertialPositionProperty2); + effector->setPropName_inertialVelocity(this->nameOfInertialVelocityProperty2); + effector->setPropName_inertialAttitude(this->nameOfInertialAttitudeProperty2); + effector->setPropName_inertialAngVelocity(this->nameOfInertialAngVelocityProperty2); + } + }; + // Terms needed for back substitution Eigen::Matrix ATheta; //!< -- rDDot_BN term for back substitution Eigen::Matrix BTheta; //!< -- omegaDot_BN term for back substitution @@ -153,14 +182,14 @@ class SpinningBodyTwoDOFStateEffector: public StateEffector, public SysModel { Eigen::Matrix3d IPrimeS2PntSc2_B; //!< [kg-m^2] body frame time derivative of inertia of inertia of upper spinning body about point Sc2 in B frame components // Spinning body properties - Eigen::Vector3d r_Sc1N_N{0.0,0.0,0.0}; //!< [m] position vector of lower spinning body center of mass Sc1 relative to the inertial frame origin N - Eigen::Vector3d r_Sc2N_N{0.0,0.0,0.0}; //!< [m] position vector of upper spinning body center of mass Sc2 relative to the inertial frame origin N - Eigen::Vector3d v_Sc1N_N{0.0,0.0,0.0}; //!< [m/s] inertial velocity vector of Sc1 relative to inertial frame - Eigen::Vector3d v_Sc2N_N{0.0,0.0,0.0}; //!< [m/s] inertial velocity vector of Sc2 relative to inertial frame - Eigen::Vector3d sigma_S1N{0.0,0.0,0.0}; //!< -- MRP attitude of frame S1 relative to inertial frame - Eigen::Vector3d sigma_S2N{0.0,0.0,0.0}; //!< -- MRP attitude of frame S2 relative to inertial frame - Eigen::Vector3d omega_S1N_S1{0.0,0.0,0.0}; //!< [rad/s] inertial lower spinning body frame angular velocity vector - Eigen::Vector3d omega_S2N_S2{0.0,0.0,0.0}; //!< [rad/s] inertial upper spinning body frame angular velocity vector + Eigen::MatrixXd* r_Sc1N_N; //!< [m] position vector of lower spinning body center of mass Sc1 relative to the inertial frame origin N + Eigen::MatrixXd* r_Sc2N_N; //!< [m] position vector of upper spinning body center of mass Sc2 relative to the inertial frame origin N + Eigen::MatrixXd* v_Sc1N_N; //!< [m/s] inertial velocity vector of Sc1 relative to inertial frame + Eigen::MatrixXd* v_Sc2N_N; //!< [m/s] inertial velocity vector of Sc2 relative to inertial frame + Eigen::MatrixXd* sigma_S1N; //!< -- MRP attitude of frame S1 relative to inertial frame + Eigen::MatrixXd* sigma_S2N; //!< -- MRP attitude of frame S2 relative to inertial frame + Eigen::MatrixXd* omega_S1N_S1; //!< [rad/s] inertial lower spinning body frame angular velocity vector + Eigen::MatrixXd* omega_S2N_S2; //!< [rad/s] inertial upper spinning body frame angular velocity vector // States double theta1 = 0.0; //!< [rad] first axis angle @@ -169,8 +198,8 @@ class SpinningBodyTwoDOFStateEffector: public StateEffector, public SysModel { double theta2Dot = 0.0; //!< [rad/s] second axis angle rate Eigen::MatrixXd* inertialPositionProperty = nullptr; //!< [m] r_N inertial position relative to system spice zeroBase/refBase Eigen::MatrixXd* inertialVelocityProperty = nullptr; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase - StateData *theta1State = nullptr; //!< -- state manager of theta1 for spinning body - StateData *theta1DotState = nullptr; //!< -- state manager of theta1Dot for spinning body + StateData* theta1State = nullptr; //!< -- state manager of theta1 for spinning body + StateData* theta1DotState = nullptr; //!< -- state manager of theta1Dot for spinning body StateData* theta2State = nullptr; //!< -- state manager of theta2 for spinning body StateData* theta2DotState = nullptr; //!< -- state manager of theta2Dot for spinning body }; From 26c9d38559e25274d9344be9891f78b1f6d43f3f Mon Sep 17 00:00:00 2001 From: Andrew Morell Date: Tue, 17 Dec 2024 15:03:01 -0700 Subject: [PATCH 09/13] add branching capabilities to thrusterDynamicEffector --- .../thrusterDynamicEffector.cpp | 32 ++++++++++++++++--- .../thrusterDynamicEffector.h | 13 +++++--- src/utilities/simIncludeThruster.py | 31 ++++++++++++++++++ 3 files changed, 67 insertions(+), 9 deletions(-) diff --git a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.cpp b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.cpp index 974580d776..36d661ab55 100644 --- a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.cpp +++ b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.cpp @@ -178,11 +178,19 @@ void ThrusterDynamicEffector::ConfigureThrustRequests(double currentTime) void ThrusterDynamicEffector::UpdateThrusterProperties() { // Save hub variables - Eigen::Vector3d r_BN_N = (Eigen::Vector3d)*this->inertialPositionProperty; - Eigen::Vector3d omega_BN_B = this->hubOmega->getState(); Eigen::MRPd sigma_BN; - sigma_BN = (Eigen::Vector3d)this->hubSigma->getState(); + Eigen::Vector3d omega_BN_B; + if (!this->stateNameOfSigma.empty()) { + omega_BN_B = this->hubOmega->getState(); + sigma_BN = (Eigen::Vector3d)this->hubSigma->getState(); + } + else { + omega_BN_B = *this->inertialAngVelocityProperty; + sigma_BN = (Eigen::Vector3d)*this->inertialAttitudeProperty; + } + Eigen::Matrix3d dcm_BN = (sigma_BN.toRotationMatrix()).transpose(); + Eigen::Vector3d r_BN_N = (Eigen::Vector3d)*this->inertialPositionProperty; // Define the variables related to which body the thruster is attached to. The F frame represents the platform body where the thruster attaches to Eigen::MRPd sigma_FN; @@ -240,6 +248,16 @@ void ThrusterDynamicEffector::linkInStates(DynParamManager& states){ } } +/*! This method is used to link the states to the thrusters + @return void + @param properties The parameter manager to collect from + */ +void ThrusterDynamicEffector::linkInProperties(DynParamManager& properties){ + this->inertialAttitudeProperty = properties.getPropertyReference(this->propName_inertialAttitude); + this->inertialAngVelocityProperty = properties.getPropertyReference(this->propName_inertialAngVelocity); + this->inertialPositionProperty = properties.getPropertyReference(this->propName_inertialPosition); +} + /*! This method computes the Forces on Torque on the Spacecraft Body. @param integTime Integration time @@ -248,7 +266,13 @@ void ThrusterDynamicEffector::linkInStates(DynParamManager& states){ void ThrusterDynamicEffector::computeForceTorque(double integTime, double timeStep) { // Save omega_BN_B - Eigen::Vector3d omegaLocal_BN_B = this->hubOmega->getState(); + Eigen::Vector3d omegaLocal_BN_B; + if (!this->stateNameOfSigma.empty()) { + omegaLocal_BN_B = this->hubOmega->getState(); + } + else { + omegaLocal_BN_B = *this->inertialAngVelocityProperty; + } // Force and torque variables Eigen::Vector3d SingleThrusterForce; diff --git a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.h b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.h index 82ef228528..61e7207581 100644 --- a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.h +++ b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.h @@ -45,13 +45,14 @@ class ThrusterDynamicEffector: public SysModel, public DynamicEffector { public: ThrusterDynamicEffector(); ~ThrusterDynamicEffector(); - void linkInStates(DynParamManager& states); - void computeForceTorque(double integTime, double timeStep); - void computeStateContribution(double integTime); - void Reset(uint64_t CurrentSimNanos); + void linkInStates(DynParamManager& states) override; + void linkInProperties(DynParamManager& properties) override; + void computeForceTorque(double integTime, double timeStep) override; + void computeStateContribution(double integTime) override; + void Reset(uint64_t CurrentSimNanos) override; void addThruster(THRSimConfig* newThruster); void addThruster(THRSimConfig* newThruster, Message* bodyStateMsg); - void UpdateState(uint64_t CurrentSimNanos); + void UpdateState(uint64_t CurrentSimNanos) override; void writeOutputMessages(uint64_t CurrentClock); bool ReadInputs(); void ConfigureThrustRequests(double currentTime); @@ -75,6 +76,8 @@ class ThrusterDynamicEffector: public SysModel, public DynamicEffector { StateData *hubSigma; //!< pointer to the hub attitude states StateData *hubOmega; //!< pointer to the hub angular velocity states Eigen::MatrixXd* inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase + Eigen::MatrixXd* inertialAttitudeProperty; //!< attitude relative to inertial frame + Eigen::MatrixXd* inertialAngVelocityProperty; //!< [rad/s] inertial angular velocity relative to inertial frame BSKLogger bskLogger; //!< -- BSK Logging diff --git a/src/utilities/simIncludeThruster.py b/src/utilities/simIncludeThruster.py index 7cadcf8fea..b785d37f6a 100755 --- a/src/utilities/simIncludeThruster.py +++ b/src/utilities/simIncludeThruster.py @@ -236,6 +236,37 @@ def addToSpacecraft(self, modelTag, thEffector, sc): return + def addToSpacecraftSubcomponent(self, modelTag, thEffector, baseEffector, segment=0): + """ + This function is for adding a thruster cluster to intermediate bodies + + Parameters + ---------- + modelTag: string + module model tag string + thEffector: thrusterEffector + thruster effector handle + baseEffector: intermediate body to be attached to, see table for compatibility guide + segment: if subcomponent is multi-body, designate which integer segment + default segment to base segment + """ + + thEffector.ModelTag = modelTag + + for key, th in list(self.thrusterList.items()): + thEffector.addThruster(th) + + # Check the type of thruster effector + thrusterType = str(type(thEffector)) + if 'ThrusterDynamicEffector' in thrusterType: + baseEffector.addDynamicEffector(thEffector, segment) + elif 'ThrusterStateEffector' in thrusterType: + baseEffector.addStateEffector(thEffector, segment) + else: + print("This isn't a thruster effector. You did something wrong.") + + return + def getNumOfDevices(self): """ Returns the number of RW devices setup. From 873c9b2734ec760e67fabcbf3adbe417b23824e9 Mon Sep 17 00:00:00 2001 From: Andrew Morell Date: Wed, 18 Dec 2024 10:15:41 -0700 Subject: [PATCH 10/13] add branching capabilities to constraintDynamicEffector --- .../constraintDynamicEffector.cpp | 53 +++++++++++++++---- .../constraintDynamicEffector.h | 15 ++++-- 2 files changed, 55 insertions(+), 13 deletions(-) diff --git a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp index cc62a868df..a9392c1cd3 100644 --- a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp +++ b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp @@ -176,6 +176,23 @@ void ConstraintDynamicEffector::linkInStates(DynParamManager& states) this->scInitCounter++; } +/*! This method is used to link properties to the thrusters + @return void + @param properties The parameter manager to collect from + */ +void ConstraintDynamicEffector::linkInProperties(DynParamManager& properties){ + if (this->scInitCounter > 1) { + bskLogger.bskLog(BSK_ERROR, "constraintDynamicEffector: tried to attach more than 2 spacecraft"); + } + + this->inertialAttitudeProperty.push_back(properties.getPropertyReference(this->propName_inertialAttitude)); + this->inertialAngVelocityProperty.push_back(properties.getPropertyReference(this->propName_inertialAngVelocity)); + this->inertialPositionProperty.push_back(properties.getPropertyReference(this->propName_inertialPosition)); + this->inertialVelocityProperty.push_back(properties.getPropertyReference(this->propName_inertialVelocity)); + + this->scInitCounter++; +} + /*! This method computes the forces on torques on each spacecraft body. @param integTime Integration time @@ -186,17 +203,35 @@ void ConstraintDynamicEffector::computeForceTorque(double integTime, double time if (this->scInitCounter == 2) { // only proceed once both spacecraft are added // alternate assigning the constraint force and torque if (this->scID == 0) { // compute all forces and torques once, assign to spacecraft 1 and store for spacecraft 2 - // - Collect states from both spacecraft - Eigen::Vector3d r_B1N_N = this->hubPosition[0]->getState(); - Eigen::Vector3d rDot_B1N_N = this->hubVelocity[0]->getState(); - Eigen::Vector3d omega_B1N_B1 = this->hubOmega[0]->getState(); + Eigen::Vector3d r_B1N_N; + Eigen::Vector3d rDot_B1N_N; + Eigen::Vector3d omega_B1N_B1; Eigen::MRPd sigma_B1N; - sigma_B1N = (Eigen::Vector3d)this->hubSigma[0]->getState(); - Eigen::Vector3d r_B2N_N = this->hubPosition[1]->getState(); - Eigen::Vector3d rDot_B2N_N = this->hubVelocity[1]->getState(); - Eigen::Vector3d omega_B2N_B2 = this->hubOmega[1]->getState(); + Eigen::Vector3d r_B2N_N; + Eigen::Vector3d rDot_B2N_N; + Eigen::Vector3d omega_B2N_B2; Eigen::MRPd sigma_B2N; - sigma_B2N = (Eigen::Vector3d)this->hubSigma[1]->getState(); + if (!this->hubPosition.empty()) { + // - Collect states from both spacecraft + r_B1N_N = this->hubPosition[0]->getState(); + rDot_B1N_N = this->hubVelocity[0]->getState(); + omega_B1N_B1 = this->hubOmega[0]->getState(); + sigma_B1N = (Eigen::Vector3d)this->hubSigma[0]->getState(); + r_B2N_N = this->hubPosition[1]->getState(); + rDot_B2N_N = this->hubVelocity[1]->getState(); + omega_B2N_B2 = this->hubOmega[1]->getState(); + sigma_B2N = (Eigen::Vector3d)this->hubSigma[1]->getState(); + } else { + // - Collect properties from parent effector + r_B1N_N = *this->inertialPositionProperty[0]; + rDot_B1N_N = *this->inertialVelocityProperty[0]; + omega_B1N_B1 = *this->inertialAngVelocityProperty[0]; + sigma_B1N = (Eigen::Vector3d)*this->inertialAttitudeProperty[0]; + r_B2N_N = *this->inertialPositionProperty[1]; + rDot_B2N_N = *this->inertialVelocityProperty[1]; + omega_B2N_B2 = *this->inertialAngVelocityProperty[1]; + sigma_B2N = (Eigen::Vector3d)*this->inertialAttitudeProperty[1]; + } // computing direction constraint psi in the N frame Eigen::Matrix3d dcm_B1N = (sigma_B1N.toRotationMatrix()).transpose(); diff --git a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h index 712e24eb68..8d327fdd7a 100644 --- a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h +++ b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h @@ -38,10 +38,11 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector { public: ConstraintDynamicEffector(); ~ConstraintDynamicEffector(); - void Reset(uint64_t CurrentSimNanos); - void linkInStates(DynParamManager& states); - void computeForceTorque(double integTime, double timeStep); - void UpdateState(uint64_t CurrentSimNanos); + void Reset(uint64_t CurrentSimNanos) override; + void linkInStates(DynParamManager& states) override; + void linkInProperties(DynParamManager& properties) override; + void computeForceTorque(double integTime, double timeStep) override; + void UpdateState(uint64_t CurrentSimNanos) override; void writeOutputStateMessage(uint64_t CurrentClock); void computeFilteredForce(uint64_t CurrentClock); void computeFilteredTorque(uint64_t CurrentClock); @@ -142,6 +143,12 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector { std::vector hubSigma; //!< parent attitude Modified Rodrigues Parameters (MRPs) std::vector hubOmega; //!< [rad/s] parent inertial angular velocity vector + // Parent body inertial properties + std::vector inertialPositionProperty; //!< [m] position relative to inertial frame + std::vector inertialVelocityProperty; //!< [m/s] velocity relative to inertial frame + std::vector inertialAttitudeProperty; //!< attitude relative to inertial frame + std::vector inertialAngVelocityProperty; //!< [rad/s] inertial angular velocity relative to inertial frame + // 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 From 0f9a6ad7348e67cc909500824cee80d772a62f2e Mon Sep 17 00:00:00 2001 From: Andrew Morell Date: Thu, 9 Jan 2025 08:57:13 -0700 Subject: [PATCH 11/13] [edit] adding unit tests to modules --- .../_UnitTest/test_thruster_dynamics_attached_body.py | 6 +++--- .../test_spinningBodyTwoDOF_branched_integrated.py | 0 2 files changed, 3 insertions(+), 3 deletions(-) create mode 100644 src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/_UnitTest/test_spinningBodyTwoDOF_branched_integrated.py diff --git a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_dynamics_attached_body.py b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_dynamics_attached_body.py index 44242298f4..f2ef52ec73 100644 --- a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_dynamics_attached_body.py +++ b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_dynamics_attached_body.py @@ -46,9 +46,9 @@ def thrusterEffectorAllTests(show_plots): # provide a unique test method name, starting with test_ def test_unitThrusters(show_plots, long_angle, lat_angle, location, rate): r""" - This unit test checks the functionality of attaching a dynamic thruster to a body other than the hub. Although the - attached body is fixed with respect to the hub, the point where the thruster is attached now has an additional - offset and a different orientation. + This unit test checks the functionality of attaching a dynamic thruster to a body other than the hub using the + messaging system. Although the attached body is fixed with respect to the hub, the point where the thruster is + attached now has an additional offset and a different orientation. The unit test sets up the thruster as normal, but then converts the direction and location to take into account the attached body for testing purposes. The thruster is set to fire for the first half of the simulation, and then turn diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/_UnitTest/test_spinningBodyTwoDOF_branched_integrated.py b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/_UnitTest/test_spinningBodyTwoDOF_branched_integrated.py new file mode 100644 index 0000000000..e69de29bb2 From c324eb5d55c9f6bc04513965f01030585d21f6cb Mon Sep 17 00:00:00 2001 From: Andrew Morell Date: Mon, 17 Feb 2025 08:42:33 -0700 Subject: [PATCH 12/13] [squash] refining unit tests --- .../thrusterDynamicEffector/thrusterDynamicEffector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.cpp b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.cpp index 36d661ab55..bf6a49ddf3 100644 --- a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.cpp +++ b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.cpp @@ -248,7 +248,7 @@ void ThrusterDynamicEffector::linkInStates(DynParamManager& states){ } } -/*! This method is used to link the states to the thrusters +/*! This method is used to link properties to the thrusters @return void @param properties The parameter manager to collect from */ From c59b07844924b09fe8e2c02747fdc02b87059583 Mon Sep 17 00:00:00 2001 From: Andrew Morell Date: Wed, 5 Mar 2025 12:28:03 -0700 Subject: [PATCH 13/13] [edit] constraint effector branching patch --- examples/scenarioConstrainedDynamics.py | 68 ++++++- examples/scenarioExtendingBoom.py | 2 +- .../_GeneralModuleFiles/dynamicEffector.h | 16 +- .../constraintDynamicEffector.cpp | 175 +++++++++++++----- .../constraintDynamicEffector.h | 33 +++- 5 files changed, 238 insertions(+), 56 deletions(-) diff --git a/examples/scenarioConstrainedDynamics.py b/examples/scenarioConstrainedDynamics.py index 97027e5ae5..00af679787 100644 --- a/examples/scenarioConstrainedDynamics.py +++ b/examples/scenarioConstrainedDynamics.py @@ -68,7 +68,7 @@ # Basilisk imports from Basilisk.architecture import messaging from Basilisk.utilities import (SimulationBaseClass, orbitalMotion, macros, RigidBodyKinematics) -from Basilisk.simulation import (spacecraft, constraintDynamicEffector, gravityEffector, svIntegrators) +from Basilisk.simulation import (spacecraft, constraintDynamicEffector, gravityEffector, svIntegrators, linearTranslationNDOFStateEffector, prescribedLinearTranslation) import matplotlib.pyplot as plt # Utility imports @@ -120,6 +120,9 @@ def run(show_plots, env): scObject2.hub.r_BcB_B = [[0.0], [0.0], [1.0]] scObject2.hub.IHubPntBc_B = [[600.0, 0.0, 0.0], [0.0, 600.0, 0.0], [0.0, 0.0, 600.0]] + # Define the spacecraft's properties + scGeometry = geometryClass() + # Add Earth gravity to the simulation if requested if env == 'Gravity': earthGravBody = gravityEffector.GravBodyData() @@ -174,6 +177,58 @@ def run(show_plots, env): scObject2.hub.v_CN_NInit = rDot_B2N_N_0 scObject2.hub.omega_BN_BInit = omega_B2N_B2_0 + # Set up translating body + translatingBodyEffector = linearTranslationNDOFStateEffector.linearTranslationNDOFStateEffector() + translatingBodyEffector.ModelTag = "translatingBodyEffector" + scObject1.addStateEffector(translatingBodyEffector) + scSim.AddModelToTask(simTaskName, translatingBodyEffector) + + translatingBody1 = linearTranslationNDOFStateEffector.translatingBody() + translatingBody1.setMass(100) + translatingBody1.setIPntFc_F([[translatingBody1.getMass() / 12 * (3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm ** 2), 0.0, 0.0], + [0.0, translatingBody1.getMass() / 12 * (scGeometry.diameterArm / 2) ** 2, 0.0], + [0.0, 0.0, translatingBody1.getMass() / 12 * (3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm ** 2)]]) + translatingBody1.setDCM_FP([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) + translatingBody1.setR_FcF_F([[0.0], [scGeometry.heightArm / 2], [0.0]]) + translatingBody1.setR_F0P_P([[0], [scGeometry.lengthHub / 2], [0]]) + translatingBody1.setFHat_P([[0], [1], [0]]) + translatingBody1.setRhoInit(0.0) + translatingBody1.setRhoDotInit(0.0) + translatingBody1.setC(400.0) + translatingBody1.setK(100.0) + translatingBodyEffector.addTranslatingBody(translatingBody1) + + translatingBody2 = linearTranslationNDOFStateEffector.translatingBody() + translatingBody2.setMass(100) + translatingBody2.setIPntFc_F([[translatingBody2.getMass() / 12 * (3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm ** 2), 0.0, 0.0], + [0.0, translatingBody2.getMass() / 12 * (scGeometry.diameterArm / 2) ** 2, 0.0], + [0.0, 0.0, translatingBody2.getMass() / 12 * ( + 3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm ** 2)]]) + translatingBody2.setDCM_FP([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) + translatingBody2.setR_FcF_F([[0.0], [scGeometry.heightArm / 2], [0.0]]) + translatingBody2.setR_F0P_P([[0], [0], [0]]) + translatingBody2.setFHat_P([[0], [1], [0]]) + translatingBody2.setRhoInit(0.0) + translatingBody2.setRhoDotInit(0.0) + translatingBody2.setC(400.0) + translatingBody2.setK(100.0) + translatingBodyEffector.addTranslatingBody(translatingBody2) + + profiler2 = prescribedLinearTranslation.PrescribedLinearTranslation() + profiler2.ModelTag = "profiler" + profiler2.setTransAccelMax(0.0005) + profiler2.setTransPosInit(translatingBody2.getRhoInit()) + profiler2.setSmoothingDuration(10) + scSim.AddModelToTask(simTaskName, profiler2) + translatingBodyEffector.translatingBodyRefInMsgs[1].subscribeTo(profiler2.linearTranslationRigidBodyOutMsg) + + translatingRigidBodyMsgData = messaging.LinearTranslationRigidBodyMsgPayload() + translatingRigidBodyMsgData.rho = scGeometry.heightArm # [m] + translatingRigidBodyMsgData.rhoDot = 0 # [m/s] + translatingRigidBodyMsg2 = messaging.LinearTranslationRigidBodyMsg().write(translatingRigidBodyMsgData) + translatingRigidBodyMsg2.this.disown() + profiler2.linearTranslationRigidBodyInMsg.subscribeTo(translatingRigidBodyMsg2) + # Create the constraint effector module constraintEffector = constraintDynamicEffector.ConstraintDynamicEffector() # Set up the constraint effector physical parameters @@ -185,7 +240,7 @@ def run(show_plots, env): constraintEffector.ModelTag = "constraintEffector" # Add the constraint to both spacecraft - scObject1.addDynamicEffector(constraintEffector) + translatingBody2.addDynamicEffector(constraintEffector) scObject2.addDynamicEffector(constraintEffector) # Add the modules to runtime call list @@ -265,6 +320,15 @@ def run(show_plots, env): return figureList +class geometryClass: + massHub = 400 + lengthHub = 3 + widthHub = 3 + heightHub = 6 + massArm = 50 + heightArm = 3 + diameterArm = 0.6 + if __name__ == "__main__": run( True, # show_plots: True or False diff --git a/examples/scenarioExtendingBoom.py b/examples/scenarioExtendingBoom.py index 57bfab7ecd..d88fa3ab87 100644 --- a/examples/scenarioExtendingBoom.py +++ b/examples/scenarioExtendingBoom.py @@ -260,7 +260,7 @@ def run(show_plots): ["arm3", translatingBodyEffector.translatingBodyConfigLogOutMsgs[2]], ["arm4", translatingBodyEffector.translatingBodyConfigLogOutMsgs[3]]] viz = vizSupport.enableUnityVisualization(scSim, dynTaskName, scBodyList - # , saveFile=fileName + , saveFile=fileName ) vizSupport.createCustomModel(viz , simBodiesToModify=[scObject.ModelTag] diff --git a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h index fdbc833408..7fec6374b4 100755 --- a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h @@ -41,19 +41,19 @@ class DynamicEffector { Eigen::Vector3d torqueExternalPntB_B = Eigen::Vector3d::Zero(); //!< [Nm] External torque applied by this effector /** setter for `stateNameOfPosition` property */ - void setStateNameOfPosition(std::string value); + virtual void setStateNameOfPosition(std::string value); /** getter for `stateNameOfPosition` property */ const std::string getStateNameOfPosition() const {return this->stateNameOfPosition; } /** setter for `stateNameOfVelocity` property */ - void setStateNameOfVelocity(std::string value); + virtual void setStateNameOfVelocity(std::string value); /** getter for `stateNameOfVelocity` property */ const std::string getStateNameOfVelocity() const { return this->stateNameOfVelocity; } /** setter for `stateNameOfSigma` property */ - void setStateNameOfSigma(std::string value); + virtual void setStateNameOfSigma(std::string value); /** getter for `stateNameOfSigma` property */ const std::string getStateNameOfSigma() const { return this->stateNameOfSigma; } /** setter for `stateNameOfOmega` property */ - void setStateNameOfOmega(std::string value); + virtual void setStateNameOfOmega(std::string value); /** getter for `stateNameOfOmega` property */ const std::string getStateNameOfOmega() const { return this->stateNameOfOmega; } /** setter for `propName_m_SC` property */ @@ -85,19 +85,19 @@ class DynamicEffector { /** getter for `propName_centerOfMassDotSC` property */ const std::string getPropName_centerOfMassDotSC() const { return this->propName_centerOfMassDotSC; } /** setter for `propName_inertialPosition` property */ - void setPropName_inertialPosition(std::string value); + virtual void setPropName_inertialPosition(std::string value); /** getter for `propName_inertialPosition` property */ const std::string getPropName_inertialPosition() const { return this->propName_inertialPosition; } /** setter for `propName_inertialVelocity` property */ - void setPropName_inertialVelocity(std::string value); + virtual void setPropName_inertialVelocity(std::string value); /** getter for `propName_inertialVelocity` property */ const std::string getPropName_inertialVelocity() const { return this->propName_inertialVelocity; } /** setter for `propName_inertialAttitude` property */ - void setPropName_inertialAttitude(std::string value); + virtual void setPropName_inertialAttitude(std::string value); /** getter for `propName_inertialAttitude` property */ const std::string getPropName_inertialAttitude() const { return this->propName_inertialAttitude; } /** setter for `propName_inertialAngVelocity` property */ - void setPropName_inertialAngVelocity(std::string value); + virtual void setPropName_inertialAngVelocity(std::string value); /** getter for `propName_inertialAngVelocity` property */ const std::string getPropName_inertialAngVelocity() const { return this->propName_inertialAngVelocity; } /** setter for `propName_vehicleGravity` property */ diff --git a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp index a9392c1cd3..bc8eb4f282 100644 --- a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp +++ b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp @@ -26,7 +26,8 @@ /*! This is the constructor, nothing to report here */ ConstraintDynamicEffector::ConstraintDynamicEffector() { - + parent1.id = 0; + parent2.id = 1; } /*! This is the destructor, nothing to report here */ @@ -35,9 +36,7 @@ ConstraintDynamicEffector::~ConstraintDynamicEffector() } -/*! This method is used to reset the module. - - */ +/*! This method is used to reset the module */ void ConstraintDynamicEffector::Reset(uint64_t CurrentSimNanos) { // check if any individual gains are not specified @@ -123,8 +122,73 @@ void ConstraintDynamicEffector::setC_a(double c_a) { } } -/*! 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. +void ConstraintDynamicEffector::setStateNameOfPosition(std::string value) { + if (!value.empty()) { + this->stateNameOfPosition.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: stateNameOfPosition variable must be a non-empty string"); + } + + +} + +void ConstraintDynamicEffector::setStateNameOfVelocity(std::string value) { + if (!value.empty()) { + this->stateNameOfVelocity.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: stateNameOfVelocity variable must be a non-empty string"); + } +} + +void ConstraintDynamicEffector::setStateNameOfSigma(std::string value) { + if (!value.empty()) { + this->stateNameOfSigma.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: stateNameOfSigma variable must be a non-empty string"); + } +} + +void ConstraintDynamicEffector::setStateNameOfOmega(std::string value) { + if (!value.empty()) { + this->stateNameOfOmega.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: stateNameOfOmega variable must be a non-empty string"); + } +} + +void ConstraintDynamicEffector::setPropName_inertialPosition(std::string value) { + if (!value.empty()) { + this->propName_inertialPosition.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: propName_inertialPosition variable must be a non-empty string"); + } +} + +void ConstraintDynamicEffector::setPropName_inertialVelocity(std::string value) { + if (!value.empty()) { + this->propName_inertialVelocity.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: propName_inertialVelocity variable must be a non-empty string"); + } +} +void ConstraintDynamicEffector::setPropName_inertialAttitude(std::string value) { + if (!value.empty()) { + this->propName_inertialAttitude.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: propName_inertialAttitude variable must be a non-empty string"); + } +} + +void ConstraintDynamicEffector::setPropName_inertialAngVelocity(std::string value) { + if (!value.empty()) { + this->propName_inertialAngVelocity.push_back(value); + } else { + bskLogger.bskLog(BSK_ERROR, "ConstraintDynamicEffector: propName_inertialAngVelocity variable must be a non-empty string"); + } +} + +/*! 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. @param wc The cut-off frequency of the low pass filter. @param h The constant digital time step. @param k The damping coefficient @@ -144,9 +208,7 @@ void ConstraintDynamicEffector::setFilter_Data(double wc, double h, double k){ } } -/*! This method allows the user to set the status of the constraint dynamic effector - -*/ +/*! This method allows the user to set the status of the constraint dynamic effector */ void ConstraintDynamicEffector::readInputMessage(){ if(this->effectorStatusInMsg.isLinked()){ DeviceStatusMsgPayload statusMsg; @@ -159,19 +221,32 @@ void ConstraintDynamicEffector::readInputMessage(){ } /*! This method allows the constraint effector to have access to the parent states - @param states The states to link */ void ConstraintDynamicEffector::linkInStates(DynParamManager& states) { if (this->scInitCounter > 1) { - bskLogger.bskLog(BSK_ERROR, "constraintDynamicEffector: tried to attach more than 2 spacecraft"); + bskLogger.bskLog(BSK_ERROR, "constraintDynamicEffector: tried to attach more than 2 parents"); } - this->hubSigma.push_back(states.getStateObject("hubSigma")); - this->hubOmega.push_back(states.getStateObject("hubOmega")); - this->hubPosition.push_back(states.getStateObject("hubPosition")); - this->hubVelocity.push_back(states.getStateObject("hubVelocity")); + this->hubSigma.push_back(states.getStateObject(this->stateNameOfSigma[scInitCounter])); + this->hubOmega.push_back(states.getStateObject(this->stateNameOfOmega[scInitCounter])); + this->hubPosition.push_back(states.getStateObject(this->stateNameOfPosition[scInitCounter])); + this->hubVelocity.push_back(states.getStateObject(this->stateNameOfVelocity[scInitCounter])); + + if (this->scInitCounter == 0) { + this->parent1.parentType = "hub"; + this->parent1.idx = 0; + this->hubCounter = 1; + } + else if (this->scInitCounter == 1) { + this->parent2.parentType = "hub"; + if (this->hubCounter == 0) { + this->parent2.idx = 0; + } else if (this->hubCounter == 1) { + this->parent2.idx = 1; + } + } this->scInitCounter++; } @@ -182,19 +257,32 @@ void ConstraintDynamicEffector::linkInStates(DynParamManager& states) */ void ConstraintDynamicEffector::linkInProperties(DynParamManager& properties){ if (this->scInitCounter > 1) { - bskLogger.bskLog(BSK_ERROR, "constraintDynamicEffector: tried to attach more than 2 spacecraft"); + bskLogger.bskLog(BSK_ERROR, "constraintDynamicEffector: tried to attach more than 2 parents"); } - this->inertialAttitudeProperty.push_back(properties.getPropertyReference(this->propName_inertialAttitude)); - this->inertialAngVelocityProperty.push_back(properties.getPropertyReference(this->propName_inertialAngVelocity)); - this->inertialPositionProperty.push_back(properties.getPropertyReference(this->propName_inertialPosition)); - this->inertialVelocityProperty.push_back(properties.getPropertyReference(this->propName_inertialVelocity)); + this->inertialAttitudeProperty.push_back(properties.getPropertyReference(this->propName_inertialAttitude[scInitCounter])); + this->inertialAngVelocityProperty.push_back(properties.getPropertyReference(this->propName_inertialAngVelocity[scInitCounter])); + this->inertialPositionProperty.push_back(properties.getPropertyReference(this->propName_inertialPosition[scInitCounter])); + this->inertialVelocityProperty.push_back(properties.getPropertyReference(this->propName_inertialVelocity[scInitCounter])); + + if (this->scInitCounter == 0) { + this->parent1.parentType = "effector"; + this->parent1.idx = 0; + this->effectorCounter = 1; + } + else if (this->scInitCounter == 1) { + this->parent2.parentType = "effector"; + if (this->effectorCounter == 0) { + this->parent2.idx = 0; + } else if (this->effectorCounter == 1) { + this->parent2.idx = 1; + } + } this->scInitCounter++; } /*! This method computes the forces on torques on each spacecraft body. - @param integTime Integration time @param timeStep Current integration time step used */ @@ -211,26 +299,29 @@ void ConstraintDynamicEffector::computeForceTorque(double integTime, double time Eigen::Vector3d rDot_B2N_N; Eigen::Vector3d omega_B2N_B2; Eigen::MRPd sigma_B2N; - if (!this->hubPosition.empty()) { - // - Collect states from both spacecraft - r_B1N_N = this->hubPosition[0]->getState(); - rDot_B1N_N = this->hubVelocity[0]->getState(); - omega_B1N_B1 = this->hubOmega[0]->getState(); - sigma_B1N = (Eigen::Vector3d)this->hubSigma[0]->getState(); - r_B2N_N = this->hubPosition[1]->getState(); - rDot_B2N_N = this->hubVelocity[1]->getState(); - omega_B2N_B2 = this->hubOmega[1]->getState(); - sigma_B2N = (Eigen::Vector3d)this->hubSigma[1]->getState(); - } else { + if (this->parent1.parentType == "hub") { + // - Collect states from parent spacecraft hub + r_B1N_N = this->hubPosition[parent1.idx]->getState(); + rDot_B1N_N = this->hubVelocity[parent1.idx]->getState(); + omega_B1N_B1 = this->hubOmega[parent1.idx]->getState(); + sigma_B1N = (Eigen::Vector3d)this->hubSigma[parent1.idx]->getState(); + } else if (this->parent2.parentType == "effector") { // - Collect properties from parent effector - r_B1N_N = *this->inertialPositionProperty[0]; - rDot_B1N_N = *this->inertialVelocityProperty[0]; - omega_B1N_B1 = *this->inertialAngVelocityProperty[0]; - sigma_B1N = (Eigen::Vector3d)*this->inertialAttitudeProperty[0]; - r_B2N_N = *this->inertialPositionProperty[1]; - rDot_B2N_N = *this->inertialVelocityProperty[1]; - omega_B2N_B2 = *this->inertialAngVelocityProperty[1]; - sigma_B2N = (Eigen::Vector3d)*this->inertialAttitudeProperty[1]; + r_B1N_N = *this->inertialPositionProperty[parent1.idx]; + rDot_B1N_N = *this->inertialVelocityProperty[parent1.idx]; + omega_B1N_B1 = *this->inertialAngVelocityProperty[parent1.idx]; + sigma_B1N = (Eigen::Vector3d)*this->inertialAttitudeProperty[parent1.idx]; + } + if (this->parent2.parentType == "hub") { + r_B2N_N = this->hubPosition[parent2.idx]->getState(); + rDot_B2N_N = this->hubVelocity[parent2.idx]->getState(); + omega_B2N_B2 = this->hubOmega[parent2.idx]->getState(); + sigma_B2N = (Eigen::Vector3d)this->hubSigma[parent2.idx]->getState(); + } else if (this->parent2.parentType == "effector") { + r_B2N_N = *this->inertialPositionProperty[parent2.idx]; + rDot_B2N_N = *this->inertialVelocityProperty[parent2.idx]; + omega_B2N_B2 = *this->inertialAngVelocityProperty[parent2.idx]; + sigma_B2N = (Eigen::Vector3d)*this->inertialAttitudeProperty[parent2.idx]; } // computing direction constraint psi in the N frame @@ -287,9 +378,8 @@ void ConstraintDynamicEffector::computeForceTorque(double integTime, double time } } -/*! This method takes the computed constraint force and torque states and outputs them to the m +/*! This method takes the computed constraint force and torque states and outputs them to the messaging system. - @param CurrentClock The current simulation time (used for time stamping) */ void ConstraintDynamicEffector::writeOutputStateMessage(uint64_t CurrentClock) @@ -307,7 +397,6 @@ void ConstraintDynamicEffector::writeOutputStateMessage(uint64_t CurrentClock) } /*! Update state method - @param CurrentSimNanos The current simulation time */ void ConstraintDynamicEffector::UpdateState(uint64_t CurrentSimNanos) @@ -321,7 +410,6 @@ void ConstraintDynamicEffector::UpdateState(uint64_t CurrentSimNanos) } /*! Filtering method to calculate filtered Constraint Force - @param CurrentClock The current simulation time (used for time stamping) */ void ConstraintDynamicEffector::computeFilteredForce(uint64_t CurrentClock) @@ -340,7 +428,6 @@ void ConstraintDynamicEffector::computeFilteredForce(uint64_t CurrentClock) } /*! Filtering method to calculate filtered Constraint Torque - @param CurrentClock The current simulation time (used for time stamping) */ void ConstraintDynamicEffector::computeFilteredTorque(uint64_t CurrentClock) diff --git a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h index 8d327fdd7a..51c0a7eb41 100644 --- a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h +++ b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h @@ -68,6 +68,22 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector { void setC_a(double c_a); /** setter for `a,b,s,c,d,e` coefficients of low pass filter */ void setFilter_Data(double wc,double h, double k); + /** setter for `stateNameOfPosition` property */ + void setStateNameOfPosition(std::string value) override; + /** setter for `stateNameOfVelocity` property */ + void setStateNameOfVelocity(std::string value) override; + /** setter for `stateNameOfSigma` property */ + void setStateNameOfSigma(std::string value) override; + /** setter for `stateNameOfOmega` property */ + void setStateNameOfOmega(std::string value) override; + /** setter for `propName_inertialPosition` property */ + void setPropName_inertialPosition(std::string value) override; + /** setter for `propName_inertialVelocity` property */ + void setPropName_inertialVelocity(std::string value) override; + /** setter for `propName_inertialAttitude` property */ + void setPropName_inertialAttitude(std::string value) override; + /** setter for `propName_inertialAngVelocity` property */ + void setPropName_inertialAngVelocity(std::string value) override; /** getter for `r_P2P1_B1Init` initial spacecraft separation */ Eigen::Vector3d getR_P2P1_B1Init() const {return this->r_P2P1_B1Init;}; @@ -96,7 +112,14 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector { // Counters and flags int scInitCounter = 0; //!< counter to kill simulation if more than two spacecraft initialized - int scID = 1; //!< 0,1 alternating spacecraft tracker to output appropriate force/torque + int scID = 1; //!< 0,1 alternating spacecraft toggle to output appropriate force/torque + struct { + int id; + int idx; + std::string parentType; + } parent1, parent2; + int hubCounter = 0; + int effectorCounter = 0; // Constraint length and direction Eigen::Vector3d r_P1B1_B1 = Eigen::Vector3d::Zero(); //!< [m] position vector from spacecraft 1 hub to its connection point P1 @@ -138,12 +161,20 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector { double T2_filtered_mag_tminus2 = 0.0; //!< Magnitude of filtered constraint torque on s/c 2 at t-2 time step // Simulation variable pointers + std::vector stateNameOfPosition; //!< state engine name of the parent rigid body inertial position vector + std::vector stateNameOfVelocity; //!< state engine name of the parent rigid body inertial velocity vector + std::vector stateNameOfSigma; //!< state engine name of the parent rigid body inertial attitude + std::vector stateNameOfOmega; //!< state engine name of the parent rigid body inertial angular velocity vector std::vector hubPosition; //!< [m] parent inertial position vector std::vector hubVelocity; //!< [m/s] parent inertial velocity vector std::vector hubSigma; //!< parent attitude Modified Rodrigues Parameters (MRPs) std::vector hubOmega; //!< [rad/s] parent inertial angular velocity vector // Parent body inertial properties + std::vector propName_inertialPosition; //!< property name of inertialPosition + std::vector propName_inertialVelocity; //!< property name of inertialVelocity + std::vector propName_inertialAttitude; //!< property name of inertialAttitude + std::vector propName_inertialAngVelocity; //!< property name of inertialAngVelocity std::vector inertialPositionProperty; //!< [m] position relative to inertial frame std::vector inertialVelocityProperty; //!< [m/s] velocity relative to inertial frame std::vector inertialAttitudeProperty; //!< attitude relative to inertial frame