Add joint velocity dynamics

src/Estimators/CMakeLists.txt

@@ -32,15 +32,19 @@ if(FRAMEWORK_COMPILE_RobotDynamicsEstimator)
                             src/StateDynamics.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}/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

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

@@ -38,7 +38,9 @@ namespace RobotDynamicsEstimator
 class FrictionTorqueDynamics : public StateDynamics
 {
     Eigen::VectorXd m_jointVelocity; /**< 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. */
 
 protected:
     Eigen::VectorXd m_tanhArgument;
@@ -61,23 +63,30 @@ class FrictionTorqueDynamics : public StateDynamics
      * |            `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;
 
+    /**
+      * Set Variablehandler object.
+      * @param variableHandler object.
+      * @return True in case of success, false otherwise.
+      */
+    bool setVariableHandler(System::VariablesHandler variableHandler) override;
+
     /**
      * Update the content of the element.
      * @return True in case of success, false otherwise.
      */
     bool update() override;
 
     /**
-     * Set the state needed to update the dynamics.
-     * @param state reference to the ukf state.
+     * Set the state of the ukf needed to update the dynamics.
+     * @param ukfState reference to the ukf state.
      * @return true if the current state has been updated correctly.
      */
-    bool setState(const Eigen::Ref<const Eigen::VectorXd> jointVelocity,
-                  const Eigen::Ref<const Eigen::VectorXd> frictionTorque);
+     void setState(const Eigen::Ref<const Eigen::VectorXd> ukfState) override;
 };
 
 } // namespace RobotDynamicsEstimator

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

@@ -0,0 +1,112 @@
+/**
+ * @file JointVelocityDynamics.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
+
+#include <memory>
+#include <map>
+
+#include <BipedalLocomotion/Math/Wrench.h>
+#include <BipedalLocomotion/RobotDynamicsEstimator/StateDynamics.h>
+#include <BipedalLocomotion/RobotDynamicsEstimator/SubModel.h>
+#include <BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h>
+
+namespace BipedalLocomotion
+{
+namespace Estimators
+{
+namespace RobotDynamicsEstimator
+{
+
+/**
+ * The JointVelocityDynamics class is a concrete implementation of the StateDynamics.
+ * 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[
+ * \ddot{s} = H^{-1} [\tau_{m} - \tau_{F} + (\sum J^T_{FT} f_{FT}) +
+ * + (\sum J^T_{ext} f_{ext})   - F^T {}^B \dot v  - h ]
+ * \f]
+ */
+
+class JointVelocityDynamics : public StateDynamics
+{
+    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. */
+    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();
+
+public:
+    /**
+     * 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);
+
+    /**
+     * 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     |
+     * |                `dT`                | `double` |                                Sampling time.                                                           |    Yes    |
+     * @return True in case of success, false otherwise.
+     */
+    bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
+
+    /**
+      * Set Variablehandler object.
+      * @param variableHandler object.
+      * @return True in case of success, false otherwise.
+      */
+    bool setVariableHandler(System::VariablesHandler variableHandler) override;
+
+    /**
+     * Update the state.
+     * @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.
+     * @return true if the current state has been updated correctly.
+     */
+     void setState(const Eigen::Ref<const Eigen::VectorXd> ukfState) override;
+
+};
+
+} // namespace RobotDynamicsEstimator
+} // namespace Estimators
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_JOINT_VELOCITY_DYNAMICS_H

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

@@ -30,16 +30,15 @@ class StateDynamics
 {
 protected:
     int m_size; /**< Size of the sub-state associate to the StateDynamics object. */
-    Eigen::VectorXd m_currentState; /**< Current state computed at the previous step. */
-    Eigen::VectorXd m_nextState; /**< Next state computed using the dynamic model. It returns the sub-state associated 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. */
     std::string m_dynamicModel;
-    double m_dT = 0.0;
     bool m_isInitialized{false}; /**< True if the state dynamics has been initialized. */
+    System::VariablesHandler m_variableHandler;
 
 public:
     /**
@@ -50,6 +49,12 @@ class StateDynamics
     virtual bool
     initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler);
 
+    /**
+      * Set Variablehandler object.
+      * @param variableHandler object
+      */
+    virtual bool setVariableHandler(System::VariablesHandler variableHandler);
+
     /**
      * Update the dynamics of the state.
      * @return True in case of success, false otherwise.
@@ -60,7 +65,7 @@ class StateDynamics
      * Get the next state m_nextState.
      * @return a const reference to the next state.
      */
-    Eigen::Ref<const Eigen::VectorXd> getNextState() const;
+    Eigen::Ref<const Eigen::VectorXd> getUpdatedSubState() const;
 
     /**
      * Get the size of the state.
@@ -69,10 +74,11 @@ class StateDynamics
     int size() const;
 
     /**
-     * Set the sampling time.
-     * @param samplingTime is the sampling time at which the filter is run.
+     * Set the state of the ukf needed to update the dynamics.
+     * @param ukfState reference to the ukf state.
+     * @return true if the current state has been updated correctly.
      */
-    void setSamplingTime(double samplingTime);
+     virtual void setState(const Eigen::Ref<const Eigen::VectorXd> ukfState) = 0;
 
     /**
      * Destructor.

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

@@ -73,47 +73,101 @@ class SubModel
     std::vector<std::string> m_externalContactList; /**< List of the additional external contacts */
 
 public:
+    /**
+     * Determines the validity of the object.
+     * @return True if the object is valid, false otherwise.
+     */
+    bool isValid() const;
+
     /**
      * Getters
      */
 
     /**
-     * @brief Access model.
+     * @brief Access m_model.
      * @return The model of the SubModel.
      */
     const iDynTree::Model& getModel() const;
 
     /**
-     * @brief Access jointListMapping.
+     * @brief Access m_jointListMapping.
      * @return the mapping between the joint indeces in the sub-model and the joint indeces in the
      * full-model.
      */
     const std::vector<int>& getJointMapping() const;
 
     /**
-     * @brief Access ftList.
+     * @brief Access m_ftList.
      * @return the list of FT objects which is the list of force/torque sensors.
      */
     const std::vector<FT>& getFTList() const;
 
     /**
-     * @brief Access accelerometerList.
+     * @brief Access m_accelerometerList.
      * @return a list of Sensor objects describing the accelerometers contained in the sub-model.
      */
     const std::vector<Sensor>& getAccelerometerList() const;
 
     /**
-     * @brief Access gyroscopeList.
+     * @brief Access m_gyroscopeList.
      * @return a list of Sensor objects describing the gyroscope contained in the sub-model.
      */
     const std::vector<Sensor>& getGyroscopeList() const;
 
     /**
-     * @brief Access externalContactList.
+     * @brief Access m_externalContactList.
      * @return a list of strings describing frame names of the external contacts for the sub-model.
      */
     const std::vector<std::string>& getExternalContactList() const;
 
+    /**
+     * @brief access the length of m_ftList.
+     * @return the number of force/torque sensors in the sub-model.
+     */
+    int getNrOfFTSensor() const;
+
+    /**
+     * @brief access the length of m_accelerometerList.
+     * @return the number of accelerometer sensors in the sub-model.
+     */
+    int getNrOfAccelerometer() const;
+
+    /**
+     * @brief access the length of m_gyroscopeList.
+     * @return the number of gyroscope sensors in the sub-model.
+     */
+    int getNrOfGyroscope() const;
+
+    /**
+     * @brief access the length of m_externalContactList.
+     * @return the number of gyroscope sensors in the sub-model.
+     */
+    int getNrOfExternalContact() const;
+
+    /**
+     * @brief Access an element of m_ftList.
+     * @return FT object associated with the specified index.
+     */
+    const FT& getFTSensor(const int index) const;
+
+    /**
+     * @brief Access an element of m_accelerometerList.
+     * @return a Sensor object corresponding to the accelerometer associated with the specified index.
+     */
+    const Sensor& getAccelerometer(const int index) const;
+
+    /**
+     * @brief Access an element of m_gyroscopeList.
+     * @return a Sensor object corresponding to the gyroscope associated with the specified index.
+     */
+    const Sensor& getGyroscope(const int index) const;
+
+    /**
+     * @brief Access an element of m_externalContactList.
+     * @return a string corresponding to the external contact frame associated with the specified index.
+     */
+    const std::string& getExternalContact(const int index) const;
+
     friend class SubModelCreator;
 };