Feature/effector branching

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

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]

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

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 properties 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;

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<SCStatesMsgPayload>* 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
 

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
@@ -173,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

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:
@@ -40,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 */
@@ -84,13 +85,21 @@ 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 */
+    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 */
+    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 */
     void setPropName_vehicleGravity(std::string value);
     /** getter for `propName_vehicleGravity` property */
@@ -113,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
 
 };

src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp

@@ -95,6 +95,18 @@ 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)
+{
+    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()
 {

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,8 @@ 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
     virtual void prependSpacecraftNameToStates();