Merge pull request #661 from isorrentino/mergeRobotDynamicsEstim/pr1

Implement robot dynamics estimator - PR1

CHANGELOG.md

@@ -39,6 +39,7 @@ All notable changes to this project are documented in this file.
 - `Catch2` is now downloaded with `FetchContent` if `BUILD_TESTING` is set to `ON` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/655)
 - The tests now uses`Catch2 v3` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/655)
 - Add the possibility to set vectorial gains in CoM IK and TSID tasks (https://github.com/ami-iit/bipedal-locomotion-framework/pull/656)
+- Add some useful methods to the `SubModel` and `SubModelKinDynWrapper` classes (https://github.com/ami-iit/bipedal-locomotion-framework/pull/661)
 
 ### Fixed
 - Return an error if an invalid `KinDynComputations` object is passed to `QPInverseKinematics::build()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/622)

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

@@ -11,6 +11,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <unordered_map>
 
 // iDynTree
 #include <iDynTree/KinDynComputations.h>
@@ -56,6 +57,7 @@ struct FT : Sensor
 
     Direction forceDirection = Direction::NotSpecified; /**< Force direction depending on which side
                                                            of the sensor is considered (+1 or -1)*/
+    std::string associatedJoint; /**< Name of the fixed joint used to represent the ft sensor in the model. */
 };
 
 /**
@@ -67,9 +69,9 @@ class SubModel
     iDynTree::Model m_model; /**< iDynTree Model object describing the submodel */
     std::vector<int> m_jointListMapping; /**< Each element contains an index describing the joint
                                             position in the full model*/
-    std::vector<FT> m_ftList; /**< List of force/torque sensors in the submodel */
-    std::vector<Sensor> m_accelerometerList; /**< List of accelerometers in the submodel */
-    std::vector<Sensor> m_gyroscopeList; /**< List of gyroscopes in the submodel */
+    std::unordered_map<std::string, FT> m_ftList; /**< List of force/torque sensors in the submodel */
+    std::unordered_map<std::string, Sensor> m_accelerometerList; /**< List of accelerometers in the submodel */
+    std::unordered_map<std::string, Sensor> m_gyroscopeList; /**< List of gyroscopes in the submodel */
     std::vector<std::string> m_externalContactList; /**< List of the additional external contacts */
 
 public:
@@ -99,21 +101,21 @@ class SubModel
 
     /**
      * @brief Access the `std::vector<FT>` list.
-     * @return the list of FT objects which is the list of force/torque sensors.
+     * @return a map of (string, FT) objects which is the list of force/torque sensors.
      */
-    const std::vector<FT>& getFTList() const;
+    const std::unordered_map<std::string, FT>& getFTList() const;
 
     /**
      * @brief Access the `std::vector<Sensor>` list of acceletometer sensors.
-     * @return a list of Sensor objects describing the accelerometers contained in the sub-model.
+     * @return a map of (string, Sensor) objects describing the accelerometers contained in the sub-model.
      */
-    const std::vector<Sensor>& getAccelerometerList() const;
+    const std::unordered_map<std::string, Sensor>& getAccelerometerList() const;
 
     /**
      * @brief Access the `std::vector<Sensor>` list of gyroscope sensors.
-     * @return a list of Sensor objects describing the gyroscope contained in the sub-model.
+     * @return a map of (string, Sensor) objects describing the gyroscope contained in the sub-model.
      */
-    const std::vector<Sensor>& getGyroscopeList() const;
+    const std::unordered_map<std::string, Sensor>& getGyroscopeList() const;
 
     /**
      * @brief Access the `std::vector<std::string>` list of frame names.
@@ -147,24 +149,49 @@ class SubModel
 
     /**
      * @brief Access an element of the force/torque sensor list.
-     * @return FT object associated with the specified index.
+     * @param is the name of the force/torque sensor.
+     * @return FT object associated with the specified name.
      */
-    const FT& getFTSensor(const int index) const;
+    const FT& getFTSensor(const std::string& name);
+
+    /**
+     * @brief hasFTSensor check if the force/torque sensor is part of the sub-model
+     * @param name is the name of the ft sensor
+     * @return true if the sensor is found, false otherwise
+     */
+    bool hasFTSensor(const std::string& name) const;
 
     /**
      * @brief Access an element of the accelerometer list.
-     * @return a Sensor object corresponding to the accelerometer associated with the specified index.
+     * @param is the name of the accelerometer.
+     * @return a Sensor object corresponding to the accelerometer associated with the specified name.
      */
-    const Sensor& getAccelerometer(const int index) const;
+    const Sensor& getAccelerometer(const std::string& name);
+
+    /**
+     * @brief hasAccelerometer check if the accelerometer is part of the sub-model
+     * @param name is the name of the accelerometer
+     * @return true if the sensor is found, false otherwise
+     */
+    bool hasAccelerometer(const std::string& name) const;
 
     /**
      * @brief Access an element of the gyroscope list.
-     * @return a Sensor object corresponding to the gyroscope associated with the specified index.
+     * @param is the name of the gyroscope.
+     * @return a Sensor object corresponding to the gyroscope associated with the specified name.
+     */
+    const Sensor& getGyroscope(const std::string& name);
+
+    /**
+     * @brief hasAccelerometer check if the gyroscope is part of the sub-model
+     * @param name is the name of the gyroscope
+     * @return true if the sensor is found, false otherwise
      */
-    const Sensor& getGyroscope(const int index) const;
+    bool hasGyroscope(const std::string& name) const;
 
     /**
      * @brief access an element of the contact frame list.
+     * @param index is the index of the external contact in the submodel.
      * @return a string corresponding to the external contact frame associated with the specified index.
      */
     const std::string& getExternalContact(const int index) const;
@@ -214,32 +241,32 @@ class SubModelCreator
      * @brief attachFTsToSubModel finds all the ft sensors connected to the specified model
      * analyzing the sensorList from the full model. Per each FT sensor creates a FT struct.
      * @param idynSubModel iDynTree Model describing one of the sub-models.
-     * @return a vector of FT structs where each FT is connected to the model (idynSubModel input
+     * @return an unordered map containing the FT structs where each FT is connected to the model (idynSubModel input
      * param)
      */
-    std::vector<FT> attachFTsToSubModel(iDynTree::Model& idynSubModel);
+    std::unordered_map<std::string, FT> attachFTsToSubModel(iDynTree::Model& idynSubModel);
 
     /**
      * @brief attachAccelerometersToSubModel finds all the accelerometer sensors connected to the
      * specified model analyzing the accListFromConfig list. Per each accelerometer sensor creates a
      * Sensor struct.
      * @param accListFromConfig list of Sensor structs.
      * @param subModel iDynTree Model object describing one of the sub-models.
-     * @return a vector of Sensor structs.
+     * @return an unordered map of Sensor structs.
      */
-    std::vector<Sensor> attachAccelerometersToSubModel(const std::vector<Sensor>& accListFromConfig,
-                                                       const iDynTree::Model& subModel);
+    std::unordered_map<std::string, Sensor> attachAccelerometersToSubModel(const std::vector<Sensor>& accListFromConfig,
+                                                                           const iDynTree::Model& subModel);
 
     /**
      * @brief attachGyroscopesToSubModel finds all the gyroscope sensors connected to the specified
      * model analyzing the gyroListFromConfig list. Per each gyroscope sensor creates a Sensor
      * struct.
      * @param gyroListFromConfig list of Sensor structs.
      * @param subModel iDynTree Model object describing one of the sub-models.
-     * @return a vector of Sensor structs.
+     * @return an unordered map of Sensor structs.
      */
-    std::vector<Sensor> attachGyroscopesToSubModel(const std::vector<Sensor>& gyroListFromConfig,
-                                                   const iDynTree::Model& subModel);
+    std::unordered_map<std::string, Sensor> attachGyroscopesToSubModel(const std::vector<Sensor>& gyroListFromConfig,
+                                                                       const iDynTree::Model& subModel);
 
     /**
      * @brief attachExternalContactsToSubModel finds all the contact frames on the specified model
@@ -265,13 +292,13 @@ class SubModelCreator
     /**
      * @brief createSubModels splits the model in SubModel objects cutting the model at the
      * force/torque sensors specified by the parameterHandler.
-     * @param ftSensorList list of Sensor structs.
+     * @param ftSensorList list of FT structs.
      * @param accList list of Sensor structs.
      * @param gyroList list of Sensor structs.
      * @param externalContacts list of strings.
      * @return a boolean value saying if the subModelList has been created correctly.
      */
-    bool createSubModels(const std::vector<Sensor>& ftSensorList,
+    bool createSubModels(const std::vector<FT>& ftSensorList,
                          const std::vector<Sensor>& accList,
                          const std::vector<Sensor>& gyroList,
                          const std::vector<std::string>& externalContacts);

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

@@ -8,7 +8,7 @@
 #ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_SUB_MODEL_KINDYN_WRAPPER_H
 #define BIPEDAL_LOCOMOTION_ESTIMATORS_SUB_MODEL_KINDYN_WRAPPER_H
 
-#include <map>
+#include <unordered_map>
 
 // iDynTree
 #include <iDynTree/Core/EigenHelpers.h>
@@ -30,6 +30,20 @@ namespace Estimators
 namespace RobotDynamicsEstimator
 {
 
+/**
+ * The enum class UpdateMode says if the updateState method should update only the
+ * information related to the robot dynamics used to compute the forward dynamics
+ * (i.e. mass matrix, generalized forces, KinDynComputation objects, jacobians
+ * associated to force/torque sensors and external contacts) or also additional
+ * objects like jacobiand associated to accelerometers, gyroscopes,
+ * bias accelerations, rotation matrices of the sensors, robot base velocity,
+ * robot base acceleration.
+ */
+enum class UpdateMode {
+    Full,
+    RobotDynamicsOnly
+};
+
 /**
  * SubModelKinDynWrapper is a concrete class and implements a wrapper of the KinDynComputation class
  * from iDynTree. The class is used to take updated the sub-model kinematics and dynamics
@@ -42,22 +56,26 @@ class SubModelKinDynWrapper
                                             kinematic and dynamic information */
     Eigen::MatrixXd m_massMatrix; /**< Mass matrix of the sub-model */
     Eigen::VectorXd m_genForces; /**< Generalized force vector */
-    std::map<const std::string, Eigen::MatrixXd> m_jacFTList; /**< Jacobians of the FT sensors */
-    std::map<const std::string, Eigen::MatrixXd> m_jacAccList; /**< Jacobians of the accelerometer
+    std::unordered_map<std::string, Eigen::MatrixXd> m_jacFTList; /**< Jacobians of the FT sensors */
+    std::unordered_map<std::string, Eigen::MatrixXd> m_jacAccList; /**< Jacobians of the accelerometer
                                                                   sensors */
-    std::map<const std::string, Eigen::MatrixXd> m_jacGyroList; /**< Jacobians of the gyroscope
+    std::unordered_map<std::string, Eigen::MatrixXd> m_jacGyroList; /**< Jacobians of the gyroscope
                                                                    sensors */
-    std::map<const std::string, Eigen::MatrixXd> m_jacContactList; /**< Jacobians of the external
+    std::unordered_map<std::string, Eigen::MatrixXd> m_jacContactList; /**< Jacobians of the external
                                                                       contacts */
-    std::map<const std::string, Eigen::VectorXd> m_dJnuList; /**< Accelerometer bias accelerations
+    std::unordered_map<std::string, Eigen::VectorXd> m_dJnuList; /**< Accelerometer bias accelerations
                                                               */
-    std::map<const std::string, manif::SO3d> m_accRworldList; /**< Rotation matrix of the
+    std::unordered_map<std::string, manif::SO3d> m_accRworldList; /**< Rotation matrix of the
                                                                  accelerometer frame wrt world */
+    std::unordered_map<std::string, manif::SE3d::Tangent> m_accVelList; /**< Acceleration of the
+                                                                 accelerometers */
     manif::SE3d::Tangent m_baseVelocity; /**< Velocity of the base of the sub-model */
     std::string m_baseFrame; /**< Name of the base frame of the sub-model */
 
     std::shared_ptr<iDynTree::KinDynComputations> m_kinDynFullModel;
 
+    manif::SE3d::Tangent m_subModelBaseAcceleration; /** Acceleration of the sub-model base. */
+
 protected:
     int m_numOfJoints; /**< Number of joints in the sub-model */
     Eigen::MatrixXd m_FTranspose; /**< It is the bottom-left block of the mass matrix, that is,
@@ -80,8 +98,15 @@ class SubModelKinDynWrapper
     /**
      * updateDynamicsVariableState updates the value of all the member variables containing
      * information about the robot kinematics and dynamics
+     * @param updateMode says if update only objects for computing the forward dynamics or also information
+     * associated to additional sensors like accelerometers/gyroscopes
      */
-    bool updateDynamicsVariableState();
+    bool updateDynamicsVariableState(UpdateMode updateMode);
+
+    /**
+     * @brief Compute the contribution of external contacts on the joint torques.
+     */
+    void computeTotalTorqueFromContacts();
 
 public:
     /**
@@ -102,10 +127,16 @@ class SubModelKinDynWrapper
     initialize(const SubModel& subModel);
 
     /**
-     * @brief updateInternalKinDynState updates the state of the KinDynWrapper object.
+     * @brief updateState updates the state of the KinDynWrapper object.
+     * @param robotBaseAcceleration is a manif::SE3d::Tangent representing the robot base acceleration.
+     * @param robotJointAcceleration is a Eigen reference to a Eigen::VectorXd containing the joint accelerations.
+     * @param updateMode says if update only objects for computing the forward dynamics or also information
+     * associated to additional sensors like accelerometers/gyroscopes
      * @return a boolean value saying if the subModelList has been created correctly.
      */
-    bool updateInternalKinDynState();
+    bool updateState(const manif::SE3d::Tangent& robotBaseAcceleration,
+                     Eigen::Ref<const Eigen::VectorXd> robotJointAcceleration,
+                     UpdateMode updateMode);
 
     /**
      * @brief forwardDynamics computes the free floaing forward dynamics
@@ -125,18 +156,13 @@ class SubModelKinDynWrapper
 
     /**
      * @brief getBaseAcceleration gets the acceleration of the sub-model base.
-     * @param robotBaseAcceleration the acceleration of the robot base.
-     * @param robotJointAcceleration the acceleration of the robot joints.
-     * @param subModelBaseAcceleration the base acceleration of the sub-model.
-     * @return a boolean value.
+     * @return subModelBaseAcceleration the acceleration of the sub-model base.
      */
-    bool getBaseAcceleration(manif::SE3d::Tangent& robotBaseAcceleration,
-                             Eigen::Ref<const Eigen::VectorXd> robotJointAcceleration,
-                             manif::SE3d::Tangent& subModelBaseAcceleration);
+    const manif::SE3d::Tangent& getBaseAcceleration();
 
     /**
      * @brief getBaseVelocity gets the acceleration of the sub-model base.
-     * @return baseVelocity the the base velocity of the sub-model.
+     * @return baseVelocity the velocity of the sub-model base.
      */
     const manif::SE3d::Tangent&
     getBaseVelocity();
@@ -204,6 +230,13 @@ class SubModelKinDynWrapper
      * @return a boolean value saying if the rotation matrix is found.
      */
     const manif::SO3d& getAccelerometerRotation(const std::string& accName) const;
+
+    /**
+     * @brief getAccelerometerVelocity access the velocity of the accelerometer specified by the input param.
+     * @param accName is the name of the accelerometer.
+     * @return the velocity of the accelerometer.
+     */
+    const manif::SE3d::Tangent& getAccelerometerVelocity(const std::string& accName);
 };
 
 } // namespace RobotDynamicsEstimator