From 442173acb80381beff09c8c9cc45a43d5cb4a6a5 Mon Sep 17 00:00:00 2001
From: Ines <ines.sorrentino10@gmail.com>
Date: Fri, 3 Mar 2023 12:26:37 +0100
Subject: [PATCH] Change StateDynamics class in Dynamics

---
 src/Estimators/CMakeLists.txt                 |  4 +-
 .../{StateDynamics.h => Dynamics.h}           | 42 +++++++-------
 .../FrictionTorqueDynamics.h                  |  4 +-
 .../JointVelocityDynamics.h                   |  4 +-
 .../RobotDynamicsEstimator/UKFModelCreator.h  | 56 -------------------
 .../ZeroVelocityDynamics.h                    |  4 +-
 src/Estimators/src/Dynamics.cpp               | 37 ++++++++++++
 src/Estimators/src/FrictionTorqueDynamics.cpp |  6 +-
 src/Estimators/src/JointVelocityDynamics.cpp  |  2 +-
 src/Estimators/src/StateDynamics.cpp          | 37 ------------
 src/Estimators/src/UKFModelCreator.cpp        | 23 --------
 src/Estimators/src/ZeroVelocityDynamics.cpp   |  6 +-
 .../FrictionTorqueDynamicsTest.cpp            |  2 +-
 .../JointVelocityDynamicsTest.cpp             |  2 +-
 .../ZeroVelocityDynamicsTest.cpp              |  2 +-
 15 files changed, 74 insertions(+), 157 deletions(-)
 rename src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/{StateDynamics.h => Dynamics.h} (55%)
 delete mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UKFModelCreator.h
 create mode 100644 src/Estimators/src/Dynamics.cpp
 delete mode 100644 src/Estimators/src/StateDynamics.cpp
 delete mode 100644 src/Estimators/src/UKFModelCreator.cpp

diff --git a/src/Estimators/CMakeLists.txt b/src/Estimators/CMakeLists.txt
index 84a5525a47..696ca97e62 100644
--- a/src/Estimators/CMakeLists.txt
+++ b/src/Estimators/CMakeLists.txt
@@ -29,14 +29,14 @@ if(FRAMEWORK_COMPILE_RobotDynamicsEstimator)
     NAME                    RobotDynamicsEstimator
     SOURCES                 src/SubModel.cpp
                             src/SubModelKinDynWrapper.cpp
-                            src/StateDynamics.cpp
+                            src/Dynamics.cpp
                             src/ZeroVelocityDynamics.cpp
                             src/FrictionTorqueDynamics.cpp
                             src/JointVelocityDynamics.cpp
     SUBDIRECTORIES          tests/RobotDynamicsEstimator
     PUBLIC_HEADERS          ${H_PREFIX}/SubModel.h
                             ${H_PREFIX}/SubModelKinDynWrapper.h
-                            ${H_PREFIX}/StateDynamics.h
+                            ${H_PREFIX}/Dynamics.h
                             ${H_PREFIX}/ZeroVelocityDynamics.h
                             ${H_PREFIX}/FrictionTorqueDynamics.h
                             ${H_PREFIX}/JointVelocityDynamics.h
diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/StateDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h
similarity index 55%
rename from src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/StateDynamics.h
rename to src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h
index ef416ea3d3..d03c26e936 100644
--- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/StateDynamics.h
+++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h
@@ -1,12 +1,12 @@
 /**
- * @file StateDynamics.h
+ * @file Dynamics.h
  * @authors Ines Sorrentino
  * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_STATEDYNAMICS_H
-#define BIPEDAL_LOCOMOTION_ESTIMATORS_STATEDYNAMICS_H
+#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_DYNAMICS_H
+#define BIPEDAL_LOCOMOTION_ESTIMATORS_DYNAMICS_H
 
 #include <memory>
 #include <string>
@@ -24,21 +24,20 @@ namespace RobotDynamicsEstimator
 {
 
 /**
- * StateDynamics describes a generic dynamic model for a UKF state.
+ * Dynamics describes a generic dynamic model for a UKF state or measurement.
  */
-class StateDynamics
+class Dynamics
 {
 protected:
-    int m_size; /**< Size of the sub-state associate to the StateDynamics object. */
-    Eigen::VectorXd m_updatedState; /**< Next state computed using the dynamic model. It returns the sub-state associated to the StateDynamics object. */
-    std::string m_description{"Generic State Dynamics Element"}; /**< String describing the content of
-                                                                 the task */
-    std::string m_name; /**< Name of state dynamics. */
-    Eigen::VectorXd m_covariances; /**< Vector of covariances of the sub-state m_nextState. */
-    std::vector<std::string> m_elements = {}; /**< Elements composing the substate m_name. This parameter is optional. */
+    int m_size; /**< Size of the variable associate to the Dynamics object. */
+    Eigen::VectorXd m_updatedVariable; /**< Updated variable computed using the dynamic model. */
+    std::string m_description{"Generic Dynamics Element"}; /**< String describing the type of the dynamics */
+    std::string m_name; /**< Name of dynamics. */
+    Eigen::VectorXd m_covariances; /**< Vector of covariances. */
+    std::vector<std::string> m_elements = {}; /**< Elements composing the variable vector. */
     std::string m_dynamicModel;
-    bool m_isInitialized{false}; /**< True if the state dynamics has been initialized. */
-    System::VariablesHandler m_variableHandler;
+    bool m_isInitialized{false}; /**< True if the dynamics has been initialized. */
+    System::VariablesHandler m_variableHandler; /**< Variable handler describing the variables and the sizes in the ukf state/measurement vector. */
 
 public:
     /**
@@ -56,16 +55,16 @@ class StateDynamics
     virtual bool setVariableHandler(System::VariablesHandler variableHandler);
 
     /**
-     * Update the dynamics of the state.
+     * Update the dynamics of the variable.
      * @return True in case of success, false otherwise.
      */
     virtual bool update();
 
     /**
-     * Get the next state m_nextState.
-     * @return a const reference to the next state.
+     * Get the next value m_updatedVariable.
+     * @return a const reference to the next variable value.
      */
-    Eigen::Ref<const Eigen::VectorXd> getUpdatedSubState() const;
+    Eigen::Ref<const Eigen::VectorXd> getUpdatedVariable() const;
 
     /**
      * Get the size of the state.
@@ -83,14 +82,11 @@ class StateDynamics
     /**
      * Destructor.
      */
-    virtual ~StateDynamics() = default;
-
-
-    // aggiungere il toString
+    virtual ~Dynamics() = default;
 };
 
 } // RobotDynamicsEstimator
 } // Estimators
 } // BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_STATEDYNAMICS_H
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_DYNAMICS_H
diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueDynamics.h
index f9315eaf2c..b84dae92a6 100644
--- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueDynamics.h
+++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueDynamics.h
@@ -9,7 +9,7 @@
 #define BIPEDAL_LOCOMOTION_ESTIMATORS_FRICTION_TORQUE_DYNAMICS_H
 
 #include <memory>
-#include <BipedalLocomotion/RobotDynamicsEstimator/StateDynamics.h>
+#include <BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h>
 
 namespace BipedalLocomotion
 {
@@ -35,7 +35,7 @@ namespace RobotDynamicsEstimator
  * \f]
  */
 
-class FrictionTorqueDynamics : public StateDynamics
+class FrictionTorqueDynamics : public Dynamics
 {
     Eigen::VectorXd m_jointVelocity; /**< Vector of joint velocities. */
     Eigen::VectorXd m_currentFrictionTorque; /**< Vector of friction torques. */
diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityDynamics.h
index fd3f6547b0..faba9c24cd 100644
--- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityDynamics.h
+++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityDynamics.h
@@ -12,7 +12,7 @@
 #include <map>
 
 #include <BipedalLocomotion/Math/Wrench.h>
-#include <BipedalLocomotion/RobotDynamicsEstimator/StateDynamics.h>
+#include <BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h>
 #include <BipedalLocomotion/RobotDynamicsEstimator/SubModel.h>
 #include <BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h>
 
@@ -33,7 +33,7 @@ namespace RobotDynamicsEstimator
  * \f]
  */
 
-class JointVelocityDynamics : public StateDynamics
+class JointVelocityDynamics : public Dynamics
 {
     std::shared_ptr<SubModelKinDynWrapper> m_kinDynWrapper; /**< object containing the kinematics and dynamics properties of a sub-model. */
     SubModel m_subModel; /**< Sub-model object storing information about the model (joints, sensors, applied wrenches) */
diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UKFModelCreator.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UKFModelCreator.h
deleted file mode 100644
index cbeb524e12..0000000000
--- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UKFModelCreator.h
+++ /dev/null
@@ -1,56 +0,0 @@
-/**
- * @file UKFModelCreator.h
- * @authors Ines Sorrentino
- * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
- * distributed under the terms of the BSD-3-Clause license.
- */
-
-#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_UKFMODELCREATOR_H
-#define BIPEDAL_LOCOMOTION_ESTIMATORS_UKFMODELCREATOR_H
-
-#include <memory>
-#include <string>
-
-#include <Eigen/Dense>
-
-// BayesFilters
-#include <BayesFilters/AdditiveStateModel.h>
-#include <BayesFilters/AdditiveMeasurementModel.h>
-
-// BLF
-#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
-#include <BipedalLocomotion/System/VariablesHandler.h>
-#include <BipedalLocomotion/RobotDynamicsEstimator/StateDynamics.h>
-#include <BipedalLocomotion/RobotDynamicsEstimator/UKFProcessModel.h>
-
-namespace BipedalLocomotion
-{
-namespace Estimators
-{
-namespace RobotDynamicsEstimator
-{
-
-class UKFModelCreator
-{
-ParametersHandler::IParametersHandler::shared_ptr stateHandler;
-ParametersHandler::IParametersHandler::shared_ptr measurementHandler;
-
-public:
-    // Initialize deve costruire il modello dello stato.
-    // Quindi deve creare un variableHandler e settarlo a UKFProcessModel
-    // creare la lista di oggetti StateDynamics e settarla a UKFProcessModel
-    // creare la matrice di covarianze e settarla a UKFProcessModel
-    bool buildStateModel(std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> stateHandler,
-                         std::vector<StateDynamics> stochasticDynamicsLsit,
-                         BipedalLocomotion::System::VariablesHandler & variableHandler);
-
-
-
-}; // class UKFModelCreator
-
-
-} // namespace RobotDynamicsEstimator
-} // namespace Estimators
-} // namespace BipedalLocomotion
-
-#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_UKFMODELCREATOR_H
diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h
index eec44fd104..d21bcdd921 100644
--- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h
+++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h
@@ -9,7 +9,7 @@
 #define BIPEDAL_LOCOMOTION_ESTIMATORS_ZERO_VELOCITY_DYNAMICS_H
 
 #include <memory>
-#include <BipedalLocomotion/RobotDynamicsEstimator/StateDynamics.h>
+#include <BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h>
 
 namespace BipedalLocomotion
 {
@@ -31,7 +31,7 @@ namespace RobotDynamicsEstimator
  * \f]
  */
 
-class ZeroVelocityDynamics : public StateDynamics
+class ZeroVelocityDynamics : public Dynamics
 {
     Eigen::VectorXd m_currentState; /**< Current state. */
 
diff --git a/src/Estimators/src/Dynamics.cpp b/src/Estimators/src/Dynamics.cpp
new file mode 100644
index 0000000000..b2d58a9c1e
--- /dev/null
+++ b/src/Estimators/src/Dynamics.cpp
@@ -0,0 +1,37 @@
+/**
+ * @file Dynamics.cpp
+ * @authors Ines Sorrentino
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h>
+
+namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator;
+using namespace BipedalLocomotion::System;
+using namespace BipedalLocomotion::ParametersHandler;
+
+bool RDE::Dynamics::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler)
+{
+    return true;
+}
+
+bool RDE::Dynamics::update()
+{
+    return true;
+}
+
+Eigen::Ref<const Eigen::VectorXd> RDE::Dynamics::getUpdatedVariable() const
+{
+    return m_updatedVariable;
+}
+
+int RDE::Dynamics::size() const
+{
+    return m_size;
+}
+
+bool RDE::Dynamics::setVariableHandler(System::VariablesHandler variableHandler)
+{
+    return true;
+}
diff --git a/src/Estimators/src/FrictionTorqueDynamics.cpp b/src/Estimators/src/FrictionTorqueDynamics.cpp
index e208d6c053..5101fde32e 100644
--- a/src/Estimators/src/FrictionTorqueDynamics.cpp
+++ b/src/Estimators/src/FrictionTorqueDynamics.cpp
@@ -83,8 +83,8 @@ bool RDE::FrictionTorqueDynamics::initialize(std::weak_ptr<const ParametersHandl
     m_currentFrictionTorque.resize(m_size);
     m_currentFrictionTorque.setZero();
 
-    m_updatedState.resize(m_size);
-    m_updatedState.setZero();
+    m_updatedVariable.resize(m_size);
+    m_updatedVariable.setZero();
 
     m_jointVelocity.resize(m_size);
     m_jointVelocity.setZero();
@@ -147,7 +147,7 @@ bool RDE::FrictionTorqueDynamics::update()
     m_dotTauF = m_k2.array() + m_k0k1.array() * m_argParenthesis.array();
 
     // \tau_{F,k+1} = \tau_{F,k} + \Delta T * \dot{\tau_{F,k}}
-    m_updatedState = m_currentFrictionTorque + m_dT * m_dotTauF;
+    m_updatedVariable = m_currentFrictionTorque + m_dT * m_dotTauF;
 
     return true;
 }
diff --git a/src/Estimators/src/JointVelocityDynamics.cpp b/src/Estimators/src/JointVelocityDynamics.cpp
index c3993b64d9..c1e76d2a3c 100644
--- a/src/Estimators/src/JointVelocityDynamics.cpp
+++ b/src/Estimators/src/JointVelocityDynamics.cpp
@@ -211,7 +211,7 @@ bool RDE::JointVelocityDynamics::update()
         return false;
     }
 
-    m_updatedState = m_jointVelocity + m_dT * m_jointAcceleration;
+    m_updatedVariable = m_jointVelocity + m_dT * m_jointAcceleration;
 
     return true;
 }
diff --git a/src/Estimators/src/StateDynamics.cpp b/src/Estimators/src/StateDynamics.cpp
deleted file mode 100644
index 64e754af60..0000000000
--- a/src/Estimators/src/StateDynamics.cpp
+++ /dev/null
@@ -1,37 +0,0 @@
-/**
- * @file SubModelKinDynWrapper.cpp
- * @authors Ines Sorrentino
- * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
- * distributed under the terms of the BSD-3-Clause license.
- */
-
-#include <BipedalLocomotion/RobotDynamicsEstimator/StateDynamics.h>
-
-namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator;
-using namespace BipedalLocomotion::System;
-using namespace BipedalLocomotion::ParametersHandler;
-
-bool RDE::StateDynamics::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler)
-{
-    return true;
-}
-
-bool RDE::StateDynamics::update()
-{
-    return true;
-}
-
-Eigen::Ref<const Eigen::VectorXd> RDE::StateDynamics::getUpdatedSubState() const
-{
-    return m_updatedState;
-}
-
-int RDE::StateDynamics::size() const
-{
-    return m_size;
-}
-
-bool RDE::StateDynamics::setVariableHandler(System::VariablesHandler variableHandler)
-{
-    return true;
-}
diff --git a/src/Estimators/src/UKFModelCreator.cpp b/src/Estimators/src/UKFModelCreator.cpp
deleted file mode 100644
index a42078dc64..0000000000
--- a/src/Estimators/src/UKFModelCreator.cpp
+++ /dev/null
@@ -1,23 +0,0 @@
-/**
- * @file UKFModelCreator.h
- * @authors Ines Sorrentino
- * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
- * distributed under the terms of the BSD-3-Clause license.
- */
-
-#include <BipedalLocomotion/RobotDynamicsEstimator/UKFModelCreator.h>
-
-namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator;
-
-void RDE::UKFStateModel::setJointStateProvider(std::shared_ptr<JointStateProvider> jointPositions)
-{
-    m_jointPosition = jointPositions;
-}
-
-//bool RDE::UKFModelCreator::makeUKFStateModel(BipedalLocomotion::System::VariablesHandler & stateHandler,
-//                                             RDE::UKFStateModel && stateModel)
-//{
-
-
-//    return true;
-//}
diff --git a/src/Estimators/src/ZeroVelocityDynamics.cpp b/src/Estimators/src/ZeroVelocityDynamics.cpp
index 37417d29f3..8afff9de58 100644
--- a/src/Estimators/src/ZeroVelocityDynamics.cpp
+++ b/src/Estimators/src/ZeroVelocityDynamics.cpp
@@ -56,8 +56,8 @@ bool RDE::ZeroVelocityDynamics::initialize(std::weak_ptr<const ParametersHandler
     m_currentState.resize(m_size);
     m_currentState.setZero();
 
-    m_updatedState.resize(m_size);
-    m_updatedState.setZero();
+    m_updatedVariable.resize(m_size);
+    m_updatedVariable.setZero();
 
     return true;
 }
@@ -81,7 +81,7 @@ bool RDE::ZeroVelocityDynamics::setVariableHandler(System::VariablesHandler vari
 bool RDE::ZeroVelocityDynamics::update()
 {
 
-    m_updatedState = m_currentState;
+    m_updatedVariable = m_currentState;
 
     return true;
 }
diff --git a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueDynamicsTest.cpp
index 55e3dea617..0e89f301b7 100644
--- a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueDynamicsTest.cpp
+++ b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueDynamicsTest.cpp
@@ -92,7 +92,7 @@ TEST_CASE("Friction Torque Dynamics")
     REQUIRE(tau_F_dynamics.update());
 
     Eigen::VectorXd nextState(covariance.size());
-    nextState = tau_F_dynamics.getUpdatedSubState();
+    nextState = tau_F_dynamics.getUpdatedVariable();
 
     Eigen::VectorXd m_tanhArgument = k1.array() * jointVel.array();
     Eigen::VectorXd expectedState = currentStateTauF.array() + dT * (k2.array() + k0.array() * k1.array() * (1 - m_tanhArgument.array().tanh().square()));
diff --git a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityDynamicsTest.cpp
index ce430e2a88..59597f6262 100644
--- a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityDynamicsTest.cpp
+++ b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityDynamicsTest.cpp
@@ -204,5 +204,5 @@ TEST_CASE("Joint Velocity Dynamics")
     REQUIRE(ds0.update());
 
     std::cout << "Updated state" << std::endl;
-    std::cout << ds0.getUpdatedSubState() << std::endl;
+    std::cout << ds0.getUpdatedVariable() << std::endl;
 }
diff --git a/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp
index be29b78ed5..be9b47d768 100644
--- a/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp
+++ b/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp
@@ -72,7 +72,7 @@ TEST_CASE("Zero Velocity Dynamics")
     REQUIRE(tau_m.update());
 
     Eigen::VectorXd nextState(covariance.size());
-    nextState = tau_m.getUpdatedSubState();
+    nextState = tau_m.getUpdatedVariable();
 
     REQUIRE(currentState == nextState);
 }