Refactoring and implemented the state dynamics and ukfstate class.

src/Estimators/CMakeLists.txt

@@ -27,29 +27,14 @@ if(FRAMEWORK_COMPILE_RobotDynamicsEstimator)
   set(H_PREFIX include/BipedalLocomotion/RobotDynamicsEstimator)
   add_bipedal_locomotion_library(
     NAME                    RobotDynamicsEstimator
-    SOURCES                 src/SubModel.cpp
-                            src/SubModelKinDynWrapper.cpp
-                            src/Dynamics.cpp
-                            src/ZeroVelocityDynamics.cpp
-                            src/FrictionTorqueDynamics.cpp
-                            src/JointVelocityDynamics.cpp
+    SOURCES                 src/SubModel.cpp src/SubModelKinDynWrapper.cpp src/Dynamics.cpp src/ZeroVelocityDynamics.cpp src/FrictionTorqueStateDynamics.cpp
+                            src/JointVelocityStateDynamics.cpp src/UkfState.cpp src/SubModelDynamics.cpp
     SUBDIRECTORIES          tests/RobotDynamicsEstimator
-    PUBLIC_HEADERS          ${H_PREFIX}/SubModel.h
-                            ${H_PREFIX}/SubModelKinDynWrapper.h
-                            ${H_PREFIX}/Dynamics.h
-                            ${H_PREFIX}/ZeroVelocityDynamics.h
-                            ${H_PREFIX}/FrictionTorqueDynamics.h
-                            ${H_PREFIX}/JointVelocityDynamics.h
-    PUBLIC_LINK_LIBRARIES   BipedalLocomotion::ParametersHandler
-                            BipedalLocomotion::TextLogging
-                            BipedalLocomotion::ManifConversions
-                            BipedalLocomotion::System
-                            BipedalLocomotion::Math
-                            iDynTree::idyntree-high-level
-                            iDynTree::idyntree-core
-                            iDynTree::idyntree-model
-                            iDynTree::idyntree-modelio
-                            Eigen3::Eigen
-                            MANIF::manif
+    PUBLIC_HEADERS          ${H_PREFIX}/SubModel.h ${H_PREFIX}/SubModelKinDynWrapper.h ${H_PREFIX}/Dynamics.h ${H_PREFIX}/ZeroVelocityDynamics.h
+                            ${H_PREFIX}/FrictionTorqueStateDynamics.h ${H_PREFIX}/JointVelocityStateDynamics.h
+                            ${H_PREFIX}/UkfState.h ${H_PREFIX}/SubModelDynamics.h
+    PUBLIC_LINK_LIBRARIES   BipedalLocomotion::ParametersHandler MANIF::manif BipedalLocomotion::System BipedalLocomotion::Math iDynTree::idyntree-high-level
+                            iDynTree::idyntree-core iDynTree::idyntree-model iDynTree::idyntree-modelio Eigen3::Eigen MANIF::manif BayesFilters::BayesFilters
+    PRIVATE_LINK_LIBRARIES  BipedalLocomotion::TextLogging BipedalLocomotion::ManifConversions
                             )
 endif()

src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h

@@ -13,8 +13,27 @@
 
 #include <Eigen/Dense>
 
+#include <BipedalLocomotion/System/Factory.h>
 #include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
 #include <BipedalLocomotion/System/VariablesHandler.h>
+#include <BipedalLocomotion/RobotDynamicsEstimator/SubModel.h>
+#include <BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h>
+
+/**
+ * BLF_REGISTER_DYNAMICS is a macro that can be used to register a Dynamics. The key of
+ * the dynamics will be the stringified version of the Dynamics C++ Type
+ * @param _model the model of the dynamics
+ * @param _baseModel the base model from which the _model inherits.
+ */
+#define BLF_REGISTER_DYNAMICS(_model, _baseModel)               \
+    static std::shared_ptr<_baseModel> _model##FactoryBuilder() \
+{                                                               \
+    return std::make_shared<_model>();                          \
+    };                                                          \
+    static std::string _model##BuilderAutoRegHook               \
+    = ::BipedalLocomotion::Estimators::                         \
+    RobotDynamicsEstimator::DynamicsFactory::registerBuilder    \
+    (#_model, _model##FactoryBuilder);
 
 namespace BipedalLocomotion
 {
@@ -37,7 +56,15 @@ class Dynamics
     std::vector<std::string> m_elements = {}; /**< Elements composing the variable vector. */
     std::string m_dynamicModel;
     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. */
+    System::VariablesHandler m_stateVariableHandler; /**< Variable handler describing the variables and the sizes in the ukf state/measurement vector. */
+    bool m_isStateVariableHandlerSet{false}; /**< True if setVariableHandler is called. */
+
+    /**
+      * Controls whether the variable handler contains the variables on which the dynamics depend.
+      * @return True in case all the dependencies are contained in the variable handler, false otherwise.
+      */
+    virtual bool checkStateVariableHandler();
+
 
 public:
     /**
@@ -49,10 +76,19 @@ class Dynamics
     initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler);
 
     /**
-      * Set Variablehandler object.
-      * @param variableHandler object
-      */
-    virtual bool setVariableHandler(System::VariablesHandler variableHandler);
+     * Finalize the Dynamics.
+     * @param handler object describing the variables in the vector for which all the dynamics are defined.
+     * @note You should call this method after you add ALL the dynamics to the variable handler.
+     * @return true in case of success, false otherwise.
+     */
+    virtual bool finalize(const System::VariablesHandler& handler);
+
+    /**
+     * Set the SubModelKinDynWrapper object.
+     * @param kinDynWrapper pointer to a SubModelKinDynWrapper object.
+     * @return True in case of success, false otherwise.
+     */
+    virtual bool setSubModels(const std::vector<SubModel>& subModelList, const std::vector<std::shared_ptr<SubModelKinDynWrapper>>& kinDynWrapperList);
 
     /**
      * Update the dynamics of the variable.
@@ -77,14 +113,31 @@ class Dynamics
      * @param ukfState reference to the ukf state.
      * @return true if the current state has been updated correctly.
      */
-     virtual void setState(const Eigen::Ref<const Eigen::VectorXd> ukfState) = 0;
+    virtual void setState(const Eigen::Ref<const Eigen::VectorXd> ukfState) = 0;
+
+    /**
+     * @brief getCovariance access the covariance `Eigen::VectorXd` associated to the variables described by this dynamics.
+     * @return the vector of covariances.
+     */
+    Eigen::Ref<const Eigen::VectorXd> getCovariance();
 
     /**
      * Destructor.
      */
     virtual ~Dynamics() = default;
 };
 
+/**
+ * DynamicsFactory implements the factory design patter for constructing a Dynamics given
+ * its model.
+ */
+class DynamicsFactory : public System::Factory<Dynamics>
+{
+
+public:
+    virtual ~DynamicsFactory() = default;
+};
+
 } // RobotDynamicsEstimator
 } // Estimators
 } // BipedalLocomotion

src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueDynamics.h → src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h

@@ -1,15 +1,16 @@
 /**
- * @file FrictionTorqueDynamics.h
+ * @file FrictionTorqueStateDynamics.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_FRICTION_TORQUE_DYNAMICS_H
-#define BIPEDAL_LOCOMOTION_ESTIMATORS_FRICTION_TORQUE_DYNAMICS_H
+#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_FRICTION_TORQUE_STATE_DYNAMICS_H
+#define BIPEDAL_LOCOMOTION_ESTIMATORS_FRICTION_TORQUE_STATE_DYNAMICS_H
 
 #include <memory>
 #include <BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h>
+#include <BipedalLocomotion/RobotDynamicsEstimator/SubModelDynamics.h>
 
 namespace BipedalLocomotion
 {
@@ -19,28 +20,35 @@ namespace RobotDynamicsEstimator
 {
 
 /**
- * The FrictionTorqueDynamics class is a concrete implementation of the StateDynamics.
+ * The FrictionTorqueDynamics class is a concrete implementation of the Dynamics.
  * Please use this element if you want to use a specific friction torque model dynamics.
  * The FrictionTorqueDynamics represents the following equation in the continuous time:
  * \f[
- * \dot{\tau_{F}} = k_{2} + k_{0} k_{1} (1 - tanh^{2} (k_{1} \dot{s}))
+ * \dot{\tau_{F}} = \ddot{s} ( k_{2} + k_{0} k_{1} (1 - tanh^{2} (k_{1} \dot{s})) )
  * \f]
  * since the friction model implemented by this class is defined as:
  * \f[
  * \tau_{F} = k_{0} tanh(k_{1} \dot{s}) + k_{2} \dot{s}
  * \f]
  * In the discrete time the dynamics is defined as:
  * \f[
- * \tau_{F,k+1} = \tau_{F,k} + \Delta T ( k_{2} + k_{0} k_{1} (1 - tanh^{2} (k_{1} \dot{s,k})) )
+ * \tau_{F,k+1} = \tau_{F,k} + \Delta T ( k_{2} + k_{0} k_{1} (1 - tanh^{2} (k_{1} \dot{s,k})) ) \ddot{s}
  * \f]
  */
 
-class FrictionTorqueDynamics : public Dynamics
+class FrictionTorqueStateDynamics : public Dynamics
 {
-    Eigen::VectorXd m_jointVelocity; /**< Vector of joint velocities. */
+    Eigen::VectorXd m_jointVelocityFullModel; /**< Vector of joint velocities. */
     Eigen::VectorXd m_currentFrictionTorque; /**< Vector of friction torques. */
     Eigen::VectorXd m_k0, m_k1, m_k2; /**< Friction parameters (see class description). */
     double m_dT; /**< Sampling time. */
+    int m_nrOfSubDynamics; /**< Number of sub-dynamics which corresponds to the number of sub-models. */
+    std::vector<std::unique_ptr<SubModelDynamics>> m_subDynamics; /**< Vector of SubModelInversDynamics objects. */
+    Eigen::VectorXd m_motorTorqueFullModel; /**< Motor torque vector of full-model. */
+    Eigen::VectorXd m_frictionTorqueFullModel; /**< Friction torque vector of full-model. */
+    std::vector<Eigen::VectorXd> m_subModelUpdatedJointAcceleration; /**< Updated joint velocity of each sub-model. */
+    Eigen::VectorXd m_jointAccelerationFullModel; /**< Vector of joint accelerations. */
+    bool m_isSubModelListSet{false}; /**< Boolean flag saying if the sub-model list has been set. */
 
 protected:
     Eigen::VectorXd m_tanhArgument;
@@ -50,6 +58,16 @@ class FrictionTorqueDynamics : public Dynamics
     Eigen::VectorXd m_dotTauF;
 
 public:
+    /*
+     * Constructor
+     */
+    FrictionTorqueStateDynamics();
+
+    /*
+     * Destructor
+     */
+    virtual ~FrictionTorqueStateDynamics();
+
     /**
      * Initialize the state dynamics.
      * @param paramHandler pointer to the parameters handler.
@@ -69,11 +87,25 @@ class FrictionTorqueDynamics : public Dynamics
     bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
 
     /**
-      * Set Variablehandler object.
-      * @param variableHandler object.
-      * @return True in case of success, false otherwise.
+     * Finalize the Dynamics.
+     * @param stateVariableHandler object describing the variables in the state vector.
+     * @note You should call this method after you add ALL the state dynamics to the state variable handler.
+     * @return true in case of success, false otherwise.
+     */
+    bool finalize(const System::VariablesHandler& stateVariableHandler) override;
+
+    /**
+     * Set the SubModelKinDynWrapper object.
+     * @param kinDynWrapper pointer to a SubModelKinDynWrapper object.
+     * @return True in case of success, false otherwise.
+     */
+    bool setSubModels(const std::vector<SubModel>& subModelList, const std::vector<std::shared_ptr<SubModelKinDynWrapper>>& kinDynWrapperList) override;
+
+    /**
+      * Controls whether the variable handler contains the variables on which the dynamics depend.
+      * @return True in case all the dependencies are contained in the variable handler, false otherwise.
       */
-    bool setVariableHandler(System::VariablesHandler variableHandler) override;
+    bool checkStateVariableHandler() override;
 
     /**
      * Update the content of the element.
@@ -89,8 +121,10 @@ class FrictionTorqueDynamics : public Dynamics
      void setState(const Eigen::Ref<const Eigen::VectorXd> ukfState) override;
 };
 
+BLF_REGISTER_DYNAMICS(FrictionTorqueStateDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics);
+
 } // namespace RobotDynamicsEstimator
 } // namespace Estimators
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FRICTION_TORQUE_DYNAMICS_H
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FRICTION_TORQUE_STATE_DYNAMICS_H

src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityDynamics.h → src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h

@@ -1,12 +1,12 @@
 /**
- * @file JointVelocityDynamics.h
+ * @file JointVelocityStateDynamics.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_JOINT_VELOCITY_DYNAMICS_H
-#define BIPEDAL_LOCOMOTION_ESTIMATORS_JOINT_VELOCITY_DYNAMICS_H
+#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_JOINT_VELOCITY_STATE_DYNAMICS_H
+#define BIPEDAL_LOCOMOTION_ESTIMATORS_JOINT_VELOCITY_STATE_DYNAMICS_H
 
 #include <memory>
 #include <map>
@@ -15,6 +15,7 @@
 #include <BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h>
 #include <BipedalLocomotion/RobotDynamicsEstimator/SubModel.h>
 #include <BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h>
+#include <BipedalLocomotion/RobotDynamicsEstimator/SubModelDynamics.h>
 
 namespace BipedalLocomotion
 {
@@ -24,7 +25,7 @@ namespace RobotDynamicsEstimator
 {
 
 /**
- * The JointVelocityDynamics class is a concrete implementation of the StateDynamics.
+ * The JointVelocityDynamics class is a concrete implementation of the Dynamics.
  * Please use this element if you want to use the robot dynaic model to update the joint dynamics.
  * The JointVelocityDynamics represents the following equation in the continuous time:
  * \f[
@@ -33,40 +34,34 @@ namespace RobotDynamicsEstimator
  * \f]
  */
 
-class JointVelocityDynamics : public Dynamics
+class JointVelocityStateDynamics : 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) */
-    std::map<std::string, Math::Wrenchd> m_FTMap; /**< The map containes names of the ft sensors and values of the wrench */
-    std::map<std::string, Math::Wrenchd> m_ExtContactMap; /**< The map containes names of the ft sensors and values of the wrench */
-    Eigen::VectorXd m_baseVelocity; /**< Velocity of the base of the sub-model. */
+    int m_nrOfSubDynamics; /**< Number of sub-dynamics which corresponds to the number of sub-models. */
+    std::vector<std::unique_ptr<SubModelDynamics>> m_subDynamics; /**< Vector of SubModelInversDynamics objects. */
+    double m_dT; /**< Sampling time. */
+    bool m_isSubModelListSet{false}; /**< Boolean flag saying if the sub-model list has been set. */
+    Eigen::VectorXd m_jointVelocityFullModel; /**< Joint velocities of full-model. */
     Eigen::VectorXd m_motorTorqueFullModel; /**< Motor torque vector of full-model. */
     Eigen::VectorXd m_frictionTorqueFullModel; /**< Friction torque vector of full-model. */
-    Eigen::VectorXd m_jointVelocityFullModel; /**< Joint velocities of full-model. */
-    Eigen::VectorXd m_motorTorque; /**< Motor torque vector of sub-model. */
-    Eigen::VectorXd m_frictionTorque; /**< Friction torque vector of sub-model. */
-    Eigen::VectorXd m_jointVelocity; /**< Joint velocities of sub-model. */
-    Eigen::VectorXd m_jointAcceleration; /**< Joint accelerations of sub-model. */
-    Eigen::VectorXd m_totalTorqueFromContacts; /**< Joint torques due to known and unknown contacts on the sub-model. */
-    Math::Wrenchd m_wrench; /**< Joint torques due to a specific contact. */
-    Eigen::VectorXd m_torqueFromContact; /**< Joint torques due to a specific contact. */
-    double m_dT; /**< Sampling time. */
-
-    void computeTotalTorqueFromContacts();
+    std::vector<Eigen::VectorXd> m_subModelUpdatedVelocity; /**< Updated joint velocity of each sub-model. */
 
 public:
+    /*
+     * Constructor
+     */
+    JointVelocityStateDynamics();
+
+    /*
+     * Destructor
+     */
+    virtual ~JointVelocityStateDynamics();
+
     /**
      * Set the SubModelKinDynWrapper object.
      * @param kinDynWrapper pointer to a SubModelKinDynWrapper object.
      * @return True in case of success, false otherwise.
      */
-    bool setKinDynWrapper(std::shared_ptr<SubModelKinDynWrapper> kinDynWrapper);
-
-    /**
-     * Set the SubModel object.
-     * @param subModel pointer to a SubModel object.
-     */
-    void setSubModel(SubModel& subModel);
+    bool setSubModels(const std::vector<SubModel>& subModelList, const std::vector<std::shared_ptr<SubModelKinDynWrapper>>& kinDynWrapperList) override;
 
     /**
      * Initialize the state dynamics.
@@ -84,11 +79,18 @@ class JointVelocityDynamics : public Dynamics
     bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
 
     /**
-      * Set Variablehandler object.
-      * @param variableHandler object.
-      * @return True in case of success, false otherwise.
+     * Finalize the Dynamics.
+     * @param stateVariableHandler object describing the variables in the state vector.
+     * @note You should call this method after you add ALL the state dynamics to the state variable handler.
+     * @return true in case of success, false otherwise.
+     */
+    bool finalize(const System::VariablesHandler& stateVariableHandler) override;
+
+    /**
+      * Controls whether the variable handler contains the variables on which the dynamics depend.
+      * @return True in case all the dependencies are contained in the variable handler, false otherwise.
       */
-    bool setVariableHandler(System::VariablesHandler variableHandler) override;
+    bool checkStateVariableHandler() override;
 
     /**
      * Update the state.
@@ -105,8 +107,10 @@ class JointVelocityDynamics : public Dynamics
 
 };
 
+BLF_REGISTER_DYNAMICS(JointVelocityStateDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics);
+
 } // namespace RobotDynamicsEstimator
 } // namespace Estimators
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_JOINT_VELOCITY_DYNAMICS_H
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_JOINT_VELOCITY_STATE_DYNAMICS_H