Add the possibility to use the feet imu in the WalkingModule

Author: GiulioRomualdi

src/WalkingModule/CMakeLists.txt

@@ -10,6 +10,6 @@ add_walking_controllers_application(
   NAME WalkingModule
   SOURCES src/main.cpp src/Module.cpp ${WalkingModule_THRIFT_GEN_FILES}
   HEADERS include/WalkingControllers/WalkingModule/Module.h
-  LINK_LIBRARIES WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities WalkingControllers::StdUtilities WalkingControllers::TimeProfiler WalkingControllers::RobotInterface WalkingControllers::KinDynWrapper WalkingControllers::TrajectoryPlanner WalkingControllers::SimplifiedModelControllers WalkingControllers::WholeBodyControllers WalkingControllers::RetargetingHelper BipedalLocomotion::VectorsCollection BipedalLocomotion::ParametersHandlerYarpImplementation ctrlLib
+  LINK_LIBRARIES WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities WalkingControllers::StdUtilities WalkingControllers::TimeProfiler WalkingControllers::RobotInterface WalkingControllers::KinDynWrapper WalkingControllers::TrajectoryPlanner WalkingControllers::SimplifiedModelControllers WalkingControllers::WholeBodyControllers WalkingControllers::RetargetingHelper BipedalLocomotion::VectorsCollection BipedalLocomotion::ParametersHandlerYarpImplementation BipedalLocomotion::ParametersHandlerYarpImplementation BipedalLocomotion::RobotInterfaceYarpImplementation ctrlLib
   SUBDIRECTORIES app)
 

src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h

@@ -10,45 +10,46 @@
 #define WALKING_MODULE_HPP
 
 // std
-#include <WalkingControllers/WholeBodyControllers/BLFIK.h>
+#include <unordered_map>
+#include <vector>
 #include <memory>
 #include <deque>
 
 // YARP
+#include <yarp/dev/PolyDriverList.h>
 #include <yarp/os/RFModule.h>
-#include <yarp/sig/Vector.h>
-
 #include <yarp/os/RpcClient.h>
+#include <yarp/sig/Vector.h>
 
-
+#include <BipedalLocomotion/RobotInterface/YarpSensorBridge.h>
+#include <BipedalLocomotion/RobotInterface/YarpHelper.h>
 #include <BipedalLocomotion/YarpUtilities/VectorsCollection.h>
 
 // iDynTree
 #include <iDynTree/Core/VectorFixSize.h>
 #include <iDynTree/ModelIO/ModelLoader.h>
+#include <iDynTree/Core/Rotation.h>
+#include <iDynTree/Core/Transform.h>
+#include <iDynTree/Model/Indices.h>
 
 // WalkingControllers library
+#include <WalkingControllers/KinDynWrapper/Wrapper.h>
+#include <WalkingControllers/RetargetingHelper/Helper.h>
 #include <WalkingControllers/RobotInterface/Helper.h>
 #include <WalkingControllers/RobotInterface/PIDHandler.h>
-#include <WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h>
-#include <WalkingControllers/TrajectoryPlanner/StableDCMModel.h>
-#include <WalkingControllers/TrajectoryPlanner/FreeSpaceEllipseManager.h>
-
 #include <WalkingControllers/SimplifiedModelControllers/DCMModelPredictiveController.h>
 #include <WalkingControllers/SimplifiedModelControllers/DCMReactiveController.h>
 #include <WalkingControllers/SimplifiedModelControllers/ZMPController.h>
-
+#include <WalkingControllers/TimeProfiler/TimeProfiler.h>
+#include <WalkingControllers/TrajectoryPlanner/FreeSpaceEllipseManager.h>
+#include <WalkingControllers/TrajectoryPlanner/StableDCMModel.h>
+#include <WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h>
+#include <WalkingControllers/WholeBodyControllers/BLFIK.h>
 #include <WalkingControllers/WholeBodyControllers/InverseKinematics.h>
 #include <WalkingControllers/WholeBodyControllers/QPInverseKinematics.h>
 #include <WalkingControllers/WholeBodyControllers/QPInverseKinematics_osqp.h>
 #include <WalkingControllers/WholeBodyControllers/QPInverseKinematics_qpOASES.h>
 
-#include <WalkingControllers/KinDynWrapper/Wrapper.h>
-
-#include <WalkingControllers/RetargetingHelper/Helper.h>
-
-#include <WalkingControllers/TimeProfiler/TimeProfiler.h>
-
 // iCub-ctrl
 #include <iCub/ctrl/filters.h>
 
@@ -65,6 +66,8 @@ namespace WalkingControllers
         enum class WalkingFSM {Idle, Configured, Preparing, Prepared, Walking, Paused, Stopped};
         WalkingFSM m_robotState{WalkingFSM::Idle}; /**< State  of the WalkingFSM. */
 
+        std::vector<BipedalLocomotion::RobotInterface::PolyDriverDescriptor> m_polyDrivers;
+
         double m_dT; /**< RFModule period. */
         double m_time; /**< Current time. */
         std::string m_robot; /**< Robot name. */
@@ -73,6 +76,7 @@ namespace WalkingControllers
         bool m_useQPIK; /**< True if the QP-IK is used. */
         bool m_useBLFIK; /**< True if the BLF-IK is used. */
         bool m_useOSQP; /**< True if osqp is used to QP-IK problem. */
+        bool m_useFeetImu{false}; /**< True if you want to use the imu on the feet. */
         bool m_dumpData; /**< True if data are saved. */
         bool m_firstRun; /**< True if it is the first run. */
         bool m_skipDCMController; /**< True if the desired ZMP should be used instead of the DCM controller. */
@@ -101,6 +105,8 @@ namespace WalkingControllers
         std::unique_ptr<RetargetingClient> m_retargetingClient; /**< Pointer to the stable DCM dynamics. */
         std::unique_ptr<TimeProfiler> m_profiler; /**< Time profiler. */
 
+        BipedalLocomotion::RobotInterface::YarpSensorBridge m_sensorBridge; /**< Helper to get the data from the robot */
+
         double m_additionalRotationWeightDesired; /**< Desired additional rotational weight matrix. */
         double m_desiredJointsWeight; /**< Desired joint weight matrix. */
         yarp::sig::Vector m_desiredJointInRadYarp; /**< Desired joint position (regularization task). */
@@ -153,6 +159,23 @@ namespace WalkingControllers
 
         yarp::os::BufferedPort<BipedalLocomotion::YarpUtilities::VectorsCollection> m_loggerPort; /**< Logger port. */
 
+        struct IMUOrientationData
+        {
+            iDynTree::Rotation IMU_R_controlledFrame;
+            iDynTree::FrameIndex frameIndex;
+            iDynTree::Rotation I_R_I_IMU;
+            iDynTree::Rotation I_R_IMU;
+            iDynTree::Rotation I_R_controlledFrame;
+        };
+
+        struct linkIMU
+        {
+            std::unordered_map<std::string, IMUOrientationData> IMUs;
+            iDynTree::Rotation averageRotation;
+        };
+
+        std::unordered_map<std::string, linkIMU> m_linksWithIMU;
+
         /**
          * Get the robot model from the resource finder and set it.
          * @param rf is the reference to a resource finder object.
@@ -264,6 +287,18 @@ namespace WalkingControllers
          */
         void applyGoalScaling(yarp::sig::Vector &plannerInput);
 
+
+        /**
+         * Configure the SensorBridge.
+         * @param rf is the reference to a resource finder object
+         * @return true in case of success and false otherwise.
+         */
+        bool configureSensorBridge(const yarp::os::Bottle& rf);
+
+        bool configureLinkWithIMU(
+            std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler,
+            const std::string& linkName);
+
     public:
 
         /**