Merge pull request #858 from AVSLab/feature/bsk-857-polymorphic-State…

…Data

Feature/bsk 857 polymorphic state data

docs/source/Support/bskReleaseNotes.rst

@@ -90,6 +90,17 @@ Version  |release|
     You have to upgrade your python ``conan`` package to be able to build Basilisk.
     Use ``python install --upgrade conan``.
 
+- Added support for subclassing ``StateData`` and overloading certain methods. This enables support for custom state
+  behavior, such as quaternions, which have size 4 but their derivative is size 3. This is done in preparation of
+  a future MuJoCo integration. Note the warning below regarding SWIG files for ``dynamicEffector`` and ``stateEffector``.
+
+  .. warning::
+
+    SWIG files for subclasses of ``dynamicEffector`` and ``stateEffector`` must now
+    ``%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.i"`` instead of
+    ``%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.h"``. See
+    ``src/simulation/dynamics/dragEffector/dragDynamicEffector.i`` for an example.
+
 
 Version 2.5.0 (Sept. 30, 2024)
 ------------------------------

examples/scenarioVariableTimeStepIntegrators.py

@@ -275,9 +275,8 @@ def run(show_plots, integratorCase, relTol, absTol):
     if integratorCase == "rkf78":
         plt.close("all")
 
-    # each test method requires a single assert method to be called
-    # this check below just makes sure no sub-test failures were found
-    return posData, figureList
+    # return some information of value for testing
+    return dataLog.times(), posData, velData, figureList
 
 
 #

src/simulation/dynamics/FuelTank/fuelTank.i

@@ -30,9 +30,8 @@ from Basilisk.architecture.swig_common_model import *
 %include "swig_conly_data.i"
 
 %include "sys_model.i"
-%include "simulation/dynamics/_GeneralModuleFiles/stateData.h"
+%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.i"
 %include "simulation/dynamics/_GeneralModuleFiles/stateEffector.h"
-%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.h"
 %include "fuelTank.h"
 
 %include "architecture/msgPayloadDefC/FuelTankMsgPayload.h"

src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.i

@@ -33,9 +33,8 @@ from Basilisk.architecture.swig_common_model import *
 
 // Instantiate templates used by example
 %include "sys_model.i"
-%include "simulation/dynamics/_GeneralModuleFiles/stateData.h"
+%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.i"
 %include "simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h"
-%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.h"
 
 %include "GravityGradientEffector.h"
 

src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.i

@@ -32,9 +32,8 @@ from Basilisk.architecture.swig_common_model import *
 %include "swig_eigen.i"
 
 %include "sys_model.i"
-%include "simulation/dynamics/_GeneralModuleFiles/stateData.h"
+%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.i"
 %include "simulation/dynamics/_GeneralModuleFiles/stateEffector.h"
-%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.h"
 %include "hingedRigidBodyStateEffector.h"
 
 %include "architecture/msgPayloadDefC/SCStatesMsgPayload.h"

src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.i

@@ -33,10 +33,9 @@ from Basilisk.architecture.swig_common_model import *
 %include "stdint.i"
 
 %include "sys_model.i"
-%include "simulation/dynamics/_GeneralModuleFiles/stateData.h"
+%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.i"
 %include "simulation/dynamics/_GeneralModuleFiles/fuelSlosh.h"
 %include "simulation/dynamics/_GeneralModuleFiles/stateEffector.h"
-%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.h"
 %include "linearSpringMassDamper.h"
 
 %pythoncode %{

src/simulation/dynamics/MtbEffector/MtbEffector.i

@@ -30,9 +30,8 @@
 %include "swig_conly_data.i"
 
 %include "sys_model.i"
-%include "simulation/dynamics/_GeneralModuleFiles/stateData.h"
+%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.i"
 %include "simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h"
-%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.h"
 %include "MtbEffector.h"
 
 %include "architecture/msgPayloadDefC/MTBCmdMsgPayload.h"

src/simulation/dynamics/NHingedRigidBodies/nHingedRigidBodyStateEffector.i

@@ -32,9 +32,8 @@ from Basilisk.architecture.swig_common_model import *
 
 
 %include "sys_model.i"
-%include "simulation/dynamics/_GeneralModuleFiles/stateData.h"
+%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.i"
 %include "simulation/dynamics/_GeneralModuleFiles/stateEffector.h"
-%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.h"
 %include "nHingedRigidBodyStateEffector.h"
 
 %pythoncode %{

src/simulation/dynamics/RadiationPressure/radiationPressure.i

@@ -28,8 +28,8 @@ from Basilisk.architecture.swig_common_model import *
 %include "std_string.i"
 %include "swig_eigen.i"
 %include "swig_conly_data.i"
+%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.i"
 %include "simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h"
-%include "simulation/dynamics/_GeneralModuleFiles/stateData.h"
 %include "sys_model.i"
 %include "radiationPressure.h"
 

src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.i

@@ -38,9 +38,8 @@ namespace std {
 }
 
 %include "sys_model.i"
-%include "simulation/dynamics/_GeneralModuleFiles/stateData.h"
+%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.i"
 %include "simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h"
-%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.h"
 %include "thrusterDynamicEffector.h"
 
 %include "simulation/dynamics/_GeneralModuleFiles/THRTimePair.h"

src/simulation/dynamics/Thrusters/thrusterStateEffector/thrusterStateEffector.i

@@ -34,9 +34,8 @@ from Basilisk.architecture.swig_common_model import *
 %include "std_vector.i"
 
 %include "sys_model.i"
-%include "simulation/dynamics/_GeneralModuleFiles/stateData.h"
+%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.i"
 %include "simulation/dynamics/_GeneralModuleFiles/stateEffector.h"
-%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.h"
 %include "simulation/dynamics/_GeneralModuleFiles/THRSimConfig.h"
 %include "thrusterStateEffector.h"
 

src/simulation/dynamics/VSCMGs/vscmgStateEffector.i

@@ -29,11 +29,9 @@ from Basilisk.architecture.swig_common_model import *
 %include "swig_conly_data.i"
 
 %include "sys_model.i"
-%include "../_GeneralModuleFiles/stateData.h"
+%include "../_GeneralModuleFiles/dynParamManager.i"
 %include "../_GeneralModuleFiles/stateEffector.h"
 %include "../_GeneralModuleFiles/dynamicEffector.h"
-%include "../_GeneralModuleFiles/dynParamManager.h"
-%include "../_GeneralModuleFiles/dynamicObject.h"
 %include "vscmgStateEffector.h"
 
 %include "architecture/msgPayloadDefC/VSCMGCmdMsgPayload.h"

src/simulation/dynamics/_GeneralModuleFiles/dynParamManager.cpp

@@ -17,177 +17,127 @@
 
  */
 
-
 #include "dynParamManager.h"
+#include <algorithm>
 #include <iostream>
 
-DynParamManager::DynParamManager()
-{
-    return;
-}
-
-DynParamManager::~DynParamManager()
+StateVector::StateVector(const StateVector& other)
 {
-    return;
+    *this = other;
 }
 
-StateData* DynParamManager::registerState(uint32_t nRow, uint32_t nCol,
-    std::string stateName)
+StateVector&
+StateVector::operator=(const StateVector& other)
 {
-    if (stateName == "") {
-        bskLogger.bskLog(BSK_ERROR, "Your state name can't be an empty string.  Come on.  You get null.");
-        return nullptr;
-    }
-
-    std::map<std::string, StateData>::iterator it;
-    it = stateContainer.stateMap.find(stateName);
-    if(it != stateContainer.stateMap.end())
-    {
-        bskLogger.bskLog(BSK_WARNING, "You created a state with the name: %s more than once.  Go ahead and don't do this.", stateName.c_str());
-        if(it->second.getRowSize() != nRow || it->second.getColumnSize() != nCol)
-        {
-            bskLogger.bskLog(BSK_ERROR, "In addition to that, you tried to change the size of the state in question.  Come on.  You get null.");
-            return nullptr;
-        }
-    }
-    else
-    {
-        Eigen::MatrixXd stateMatrix;
-        stateMatrix.resize(nRow, nCol);
-        StateData newState(stateName, stateMatrix);
-        stateContainer.stateMap.insert(std::pair<std::string, StateData>
-                              (stateName, newState));
-        it = stateContainer.stateMap.find(stateName);
+    stateMap.clear();
+    for (const auto& [key, value] : other.stateMap) {
+        stateMap.emplace(key, value->clone());
     }
-    return (&(it->second));
+    return *this;
 }
 
-StateData* DynParamManager::getStateObject(std::string stateName)
+void StateVector::setStates(const StateVector& operand)
 {
-    StateData *statePtr;
-    std::map<std::string, StateData>::iterator it;
-
-    statePtr = nullptr;
-    it = stateContainer.stateMap.find(stateName);
-    if(it != stateContainer.stateMap.end())
-    {
-        statePtr = &(it->second);
+    for (auto [it1, it2] = std::tuple{std::begin(this->stateMap), std::begin(operand.stateMap)};
+         it1 != std::end(this->stateMap) && it2 != std::end(operand.stateMap);
+         ++it1, ++it2) {
+        it1->second->setState(it2->second->getState());
     }
-
-    if (statePtr == nullptr)
-    {
-        /*  The requested state could not be found.
-            Either the state name was miss-spelled, or the state simply
-            doesn't exit in the current simulaiton setup (i.e. asking for the
-            hub attitude in a translation only simulation setup */
-        bskLogger.bskLog(BSK_WARNING, "You requested this non-existent state name: %s You either miss-typed the stateName, or you asked for a state that doesn't exist in your simulation setup. stateName = ", stateName.c_str());
-    }
-
-    return(statePtr);
 }
 
-StateVector DynParamManager::getStateVector()
+void StateVector::addStates(const StateVector& operand)
 {
-    return(stateContainer);
+    for (auto [it1, it2] = std::tuple{std::begin(this->stateMap), std::begin(operand.stateMap)};
+         it1 != std::end(this->stateMap) && it2 != std::end(operand.stateMap);
+         ++it1, ++it2) {
+        it1->second->addState(*it2->second);
+    }
 }
 
-void DynParamManager::updateStateVector(const StateVector & newState)
+void StateVector::scaleStates(double scaleFactor)
 {
-
-    std::map<std::string, StateData>::iterator it;
-    std::map<std::string, StateData>::const_iterator inIt;
-    for (it = stateContainer.stateMap.begin(), inIt = newState.stateMap.begin();
-         it != stateContainer.stateMap.end() && inIt != newState.stateMap.end(); it++, inIt++)
-    {
-        it->second.setState(inIt->second.getState());
-
+    for (const auto& [key, value] : stateMap) {
+        value->scaleState(scaleFactor);
     }
 }
 
-void DynParamManager::propagateStateVector(double dt)
+void StateVector::propagateStates(double dt)
 {
-    std::map<std::string, StateData>::iterator it;
-    for (it = stateContainer.stateMap.begin();
-         it != stateContainer.stateMap.end(); it++)
-    {
-        it->second.propagateState(dt);
-
+    for (const auto& [key, value] : stateMap) {
+        value->propagateState(dt);
     }
 }
 
-StateVector StateVector::operator+(const StateVector& operand)
+StateData* DynParamManager::getStateObject(std::string stateName)
 {
-    std::map<std::string, StateData>::iterator it;
-    std::map<std::string, StateData>::const_iterator opIt;
-    StateVector outVector;
-    for (it = stateMap.begin(), opIt = operand.stateMap.begin();
-         it != stateMap.end() && opIt != operand.stateMap.end(); it++, opIt++)
-    {
-        StateData newState = it->second + opIt->second;
-        outVector.stateMap.insert(std::pair<std::string, StateData>
-                                  (it->first, newState));
-
+    if (stateContainer.stateMap.count(stateName) > 0) {
+        return stateContainer.stateMap.at(stateName).get();
     }
-    return outVector;
+
+    /*  The requested state could not be found.
+        Either the state name was miss-spelled, or the state simply
+        doesn't exit in the current simulaiton setup (i.e. asking for the
+        hub attitude in a translation only simulation setup */
+    bskLogger.bskLog(
+        BSK_WARNING,
+        "You requested this non-existent state name: %s You either miss-typed the stateName, or "
+        "you asked for a state that doesn't exist in your simulation setup.",
+        stateName.c_str());
+
+    return nullptr;
 }
 
-StateVector StateVector::operator*(double scaleFactor)
+void DynParamManager::updateStateVector(const StateVector& newState)
 {
-    StateVector outVector;
-    std::map<std::string, StateData>::iterator it;
-    for (it = stateMap.begin(); it != stateMap.end(); it++)
-    {
-        outVector.stateMap.insert(std::pair<std::string, StateData>
-                                  (it->first, it->second*scaleFactor));
-
-    }
-
-    return outVector;
+    this->stateContainer.setStates(newState);
 }
 
+void DynParamManager::propagateStateVector(double dt) { this->stateContainer.propagateStates(dt); }
+
 Eigen::MatrixXd* DynParamManager::createProperty(std::string propName,
-    const Eigen::MatrixXd & propValue)
+                                                 const Eigen::MatrixXd& propValue)
 {
     std::map<std::string, Eigen::MatrixXd>::iterator it;
     it = dynProperties.find(propName);
-    if(it == dynProperties.end())
-    {
-        dynProperties.insert(std::pair<std::string, Eigen::MatrixXd>
-                             (propName, propValue));
+    if (it == dynProperties.end()) {
+        dynProperties.insert(std::pair<std::string, Eigen::MatrixXd>(propName, propValue));
     }
-    else{
-        bskLogger.bskLog(BSK_WARNING, "You created the dynamic property: %s more than once.  You shouldn't be doing that.", propName.c_str());
+    else {
+        bskLogger.bskLog(
+            BSK_WARNING,
+            "You created the dynamic property: %s more than once.  You shouldn't be doing that.",
+            propName.c_str());
         it->second = propValue;
     }
-    return(&(dynProperties.find(propName)->second));
+    return (&(dynProperties.find(propName)->second));
 }
 
 Eigen::MatrixXd* DynParamManager::getPropertyReference(std::string propName)
 {
     std::map<std::string, Eigen::MatrixXd>::iterator it;
     it = dynProperties.find(propName);
-    if(it == dynProperties.end())
-    {
-        bskLogger.bskLog(BSK_ERROR, "You requested the property: %s which doesn't exist.  Null returned.", propName.c_str());
+    if (it == dynProperties.end()) {
+        bskLogger.bskLog(BSK_ERROR,
+                         "You requested the property: %s which doesn't exist.  Null returned.",
+                         propName.c_str());
         return nullptr;
     }
-    else
-    {
-        return(&(it->second));
+    else {
+        return (&(it->second));
     }
 }
 
-void DynParamManager::setPropertyValue(const std::string propName,
-                      const Eigen::MatrixXd & propValue)
+void DynParamManager::setPropertyValue(const std::string propName, const Eigen::MatrixXd& propValue)
 {
     std::map<std::string, Eigen::MatrixXd>::iterator it;
     it = dynProperties.find(propName);
-    if(it == dynProperties.end())
-    {
-        bskLogger.bskLog(BSK_ERROR, "You tried to set the property value for: %s which has not been created yet. I can't do that.", propName.c_str());
+    if (it == dynProperties.end()) {
+        bskLogger.bskLog(BSK_ERROR,
+                         "You tried to set the property value for: %s which has not been created "
+                         "yet. I can't do that.",
+                         propName.c_str());
     }
-    else
-    {
+    else {
         it->second = propValue;
     }
 }