Implement the Dynamics base class, the dynamics classes for the proce…

…ss model, the ukfstate model class.

cmake/BipedalLocomotionFrameworkDependencies.cmake

@@ -86,6 +86,10 @@ find_package(robometry 1.1.0 QUIET)
 checkandset_dependency(robometry MINIMUM_VERSION 1.1.0)
 dependency_classifier(robometry MINIMUM_VERSION 1.1.0 IS_USED ${FRAMEWORK_USE_robometry})
 
+find_package(BayesFilters QUIET)
+checkandset_dependency(BayesFilters)
+dependency_classifier(BayesFilters IS_USED ${FRAMEWORK_USE_BayesFilters})
+
 # required only for some tests
 find_package(icub-models 1.23.3 QUIET)
 checkandset_dependency(icub-models)
@@ -170,7 +174,7 @@ framework_dependent_option(FRAMEWORK_COMPILE_FloatingBaseEstimators
 
 framework_dependent_option(FRAMEWORK_COMPILE_RobotDynamicsEstimator
   "Compile RobotDynamicsEstimator libraries?" ON
-  "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_System;FRAMEWORK_COMPILE_ManifConversions;FRAMEWORK_USE_manif" OFF)
+  "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_System;FRAMEWORK_COMPILE_ManifConversions;FRAMEWORK_USE_manif;FRAMEWORK_USE_BayesFilters" OFF)
 
 framework_dependent_option(FRAMEWORK_COMPILE_ManifConversions
   "Compile manif Conversions libraries?" ON

src/Estimators/CMakeLists.txt

@@ -27,19 +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
+    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
-    PUBLIC_LINK_LIBRARIES   BipedalLocomotion::ParametersHandler
-                            BipedalLocomotion::TextLogging
-                            BipedalLocomotion::ManifConversions
-                            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

@@ -0,0 +1,198 @@
+/**
+ * @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_DYNAMICS_H
+#define BIPEDAL_LOCOMOTION_ESTIMATORS_DYNAMICS_H
+
+#include <memory>
+#include <string>
+
+#include <Eigen/Dense>
+
+#include <BipedalLocomotion/System/Factory.h>
+#include <BipedalLocomotion/System/Advanceable.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
+{
+namespace Estimators
+{
+namespace RobotDynamicsEstimator
+{
+
+/**
+ * @brief The UKFInput struct represents the input of the ukf needed to update the dynamics.
+ */
+struct UKFInput
+{
+    Eigen::VectorXd robotJointPositions; /**< Vector of joint positions. */
+    manif::SE3d robotBasePose; /**< Robot base position and orientation. */
+    manif::SE3d::Tangent robotBaseVelocity; /**< Robot base velocity. */
+    manif::SE3d::Tangent robotBaseAcceleration; /**< Robot base acceleration. */
+};
+
+/**
+ * UkfInputProvider describes the provider for the inputs of the ukf
+ */
+class UkfInputProvider : public System::Advanceable<UKFInput, UKFInput>
+{
+private:
+    UKFInput m_ukfInput;
+
+public:
+    /**
+     * Get the inputes
+     * @return A struct containing the inputs for the ukf
+     */
+    const UKFInput& getOutput() const override;
+
+    /**
+     * Set the state which represents the state of the provider.
+     * @param input is a struct containing the input of the ukf.
+     */
+    bool setInput(const UKFInput& input) override;
+
+    /**
+     * @brief Advance the internal state. This may change the value retrievable from getOutput().
+     * @return True if the advance is successfull.
+     */
+    bool advance() override;
+
+    /**
+     * @brief Determines the validity of the object retrieved with getOutput()
+     * @return True if the object is valid, false otherwise.
+     */
+    bool isOutputValid() const override;
+
+}; // class UkfProcessInputProvider
+
+/**
+ * Dynamics describes a generic dynamic model for a UKF state or measurement.
+ */
+class Dynamics
+{
+protected:
+    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 dynamics has been initialized. */
+    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. */
+    UKFInput m_ukfInput;
+
+    /**
+      * 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:
+    /**
+     * Initialize the task.
+     * @param paramHandler pointer to the parameters handler.
+     * @return True in case of success, false otherwise.
+     */
+    virtual bool
+    initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler);
+
+    /**
+     * 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.
+     * @return True in case of success, false otherwise.
+     */
+    virtual bool update();
+
+    /**
+     * Get the next value m_updatedVariable.
+     * @return a const reference to the next variable value.
+     */
+    Eigen::Ref<const Eigen::VectorXd> getUpdatedVariable() const;
+
+    /**
+     * Get the size of the state.
+     * @return the size of the state.
+     */
+    int size() const;
+
+    /**
+     * Set the state of the ukf needed to update the dynamics.
+     * @param ukfState reference to the ukf state.
+     */
+    virtual void setState(const Eigen::Ref<const Eigen::VectorXd> ukfState) = 0;
+
+    /**
+     * Set a `UKFInput` object.
+     * @param ukfInput reference to the UKFInput struct.
+     */
+    virtual void setInput(const UKFInput & ukfInput) = 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
+
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_DYNAMICS_H

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

@@ -0,0 +1,135 @@
+/**
+ * @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_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
+{
+namespace Estimators
+{
+namespace RobotDynamicsEstimator
+{
+
+/**
+ * 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}} = \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})) ) \ddot{s}
+ * \f]
+ */
+
+class FrictionTorqueStateDynamics : public Dynamics
+{
+    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 acceleration 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;
+    Eigen::VectorXd m_tanh;
+    Eigen::VectorXd m_k0k1;
+    Eigen::VectorXd m_argParenthesis;
+    Eigen::VectorXd m_dotTauF;
+
+public:
+    /*
+     * Constructor
+     */
+    FrictionTorqueStateDynamics();
+
+    /*
+     * Destructor
+     */
+    virtual ~FrictionTorqueStateDynamics();
+
+    /**
+     * Initialize the state dynamics.
+     * @param paramHandler pointer to the parameters handler.
+     * @note the following parameters are required by the class
+     * |           Parameter Name           |   Type   |                                       Description                                                       | Mandatory |
+     * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:|
+     * |               `name`               | `string` |   Name of the state contained in the `VariablesHandler` describing the state associated to this dynamics|    Yes    |
+     * |            `covariance`            | `vector` |                                Process covariances                                                      |    Yes    |
+     * |           `dynamic_model`          | `string` |               Type of dynamic model describing the state dynamics.                                      |    Yes    |
+     * |             `elements`             | `vector` |  Vector of strings describing the list of sub variables composing the state associated to this dynamics.|    No     |
+     * |            `friction_k0`           | `vector` |  Vector of coefficients k0. For more info check the class description.                                  |    Yes    |
+     * |            `friction_k1`           | `vector` |  Vector of coefficients k1. For more info check the class description.                                  |    Yes    |
+     * |            `friction_k2`           | `vector` |  Vector of coefficients k2. For more info check the class description.                                  |    Yes    |
+     * |                `dT`                | `double` |                                Sampling time.                                                           |    Yes    |
+     * @return True in case of success, false otherwise.
+     */
+    bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
+
+    /**
+     * 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 checkStateVariableHandler() override;
+
+    /**
+     * Update the content of the element.
+     * @return True in case of success, false otherwise.
+     */
+    bool update() override;
+
+    /**
+     * Set the state of the ukf needed to update the dynamics.
+     * @param ukfState reference to the ukf state.
+     */
+     void setState(const Eigen::Ref<const Eigen::VectorXd> ukfState) override;
+
+     /**
+      * Set a `UKFInput` object.
+      * @param ukfInput reference to the UKFInput struct.
+      */
+      void setInput(const UKFInput & ukfInput) override;
+};
+
+BLF_REGISTER_DYNAMICS(FrictionTorqueStateDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics);
+
+} // namespace RobotDynamicsEstimator
+} // namespace Estimators
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FRICTION_TORQUE_STATE_DYNAMICS_H