Change StateDynamics class in Dynamics

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

src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/StateDynamics.h → 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

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. */

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) */

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. */
 

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;
+}

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

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

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

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()));

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

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);
 }