Add the possibility to use the feet IMU in BLFIK

cmake/WalkingControllersDependencies.cmake

@@ -10,7 +10,8 @@ find_package(Eigen3 3.2.92 REQUIRED)
 find_package(UnicyclePlanner 0.5.2 REQUIRED)
 find_package(OsqpEigen 0.4.0 REQUIRED)
 find_package(qpOASES REQUIRED)
-find_package(BipedalLocomotionFramework 0.9.0
+find_package(BipedalLocomotionFramework 0.10.0
   COMPONENTS VectorsCollection IK ParametersHandlerYarpImplementation
              ContinuousDynamicalSystem ManifConversions
-             ParametersHandlerYarpImplementation REQUIRED)
+             ParametersHandlerYarpImplementation
+             RobotInterfaceYarpImplementation REQUIRED)

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/app/robots/iCubGenova09/dcm_walking/common/sensorBridge.ini

@@ -0,0 +1,29 @@
+check_for_nan                                    false
+stream_joint_states                              false
+stream_cartesian_wrenches                        false
+stream_inertials                                 true
+
+[InertialSensors]
+orientation_sensors_list	(l_foot_front_ft_eul, l_foot_rear_ft_eul, r_foot_front_ft_eul, r_foot_rear_ft_eul)
+
+[LEFT_FOOT]
+description     left_foot
+remote_port_name    /icub/left_foot/imu
+local_prefix        walking-coordinator
+local_port_name_postfix  /left_foot/imu
+controlled_frame_name   l_sole
+imu_names               (l_foot_front_ft_eul, l_foot_rear_ft_eul)
+frame_names             (l_foot_front_ft_sensor, l_foot_rear_ft_sensor)
+
+[RIGHT_FOOT]
+description     right_foot
+remote_port_name    /icub/right_foot/imu
+local_prefix        walking-coordinator
+local_port_name_postfix  /right_foot/imu
+controlled_frame_name   r_sole
+imu_names               (r_foot_front_ft_eul, r_foot_rear_ft_eul)
+frame_names             (r_foot_front_ft_sensor, r_foot_rear_ft_sensor)
+
+[MULTIPLE_ANALOG_SENSORS_REMAPPER]
+description     remapper
+orientation_sensors_names (l_foot_front_ft_eul, l_foot_rear_ft_eul, r_foot_front_ft_eul, r_foot_rear_ft_eul)

src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini

@@ -6,7 +6,7 @@ kp                              (5.0, 5.0, 5.0,
                                  5.0, 5.0, 5.0, 5.0, 5.0, 5.0)
 
 states          ("stance", "walking")
-sampling_time   0.01
+sampling_time   0.002
 settling_time   0.5
 
 [stance]

src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini

@@ -4,7 +4,7 @@ kp_angular                      5.0
 
 
 states          ("stance", "walking")
-sampling_time   0.01
+sampling_time   0.002
 settling_time   0.5
 
 [stance]

src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini

@@ -39,7 +39,7 @@ goal_port_suffix                    /goal:i
 goal_port_scaling                   (0.5, 1.0, 0.5)
 
 # How much in advance the planner should be called. The time is in seconds
-planner_advance_time_in_s           0.08
+planner_advance_time_in_s           0.02
 
 # general parameters
 [GENERAL]
@@ -56,6 +56,8 @@ use_com_retargeting     0
 # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link
 height_reference_frame  root_link
 
+
+
 # include robot control parameters
 [include ROBOT_CONTROL "./dcm_walking/iFeel_joint_retargeting/robotControl.ini"]
 
@@ -97,3 +99,6 @@ height_reference_frame  root_link
 
 # retargeting
 [include RETARGETING "./dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini"]
+
+# include sensor bridge parameters
+[include SENSOR_BRIDGE "./dcm_walking/common/sensorBridge.ini"]

src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini

@@ -39,17 +39,19 @@ goal_port_suffix                    /goal:i
 goal_port_scaling                   (0.5, 1.0, 0.5)
 
 # How much in advance the planner should be called. The time is in seconds
-planner_advance_time_in_s           0.08
+planner_advance_time_in_s           0.02
 
 # general parameters
 [GENERAL]
 name                    walking-coordinator
 # height of the com
 com_height              0.565
 # sampling time
-sampling_time           0.01
+sampling_time           0.002
 # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link
 height_reference_frame  root_link
+use_left_foot_imu       true
+use_right_foot_imu       true
 
 # include robot control parameters
 [include ROBOT_CONTROL "./dcm_walking/joypad_control/robotControl.ini"]
@@ -89,3 +91,6 @@ height_reference_frame  root_link
 
 # include lower PID parameters
 [include PID "./dcm_walking/common/pidParams.ini"]
+
+# include sensor bridge parameters
+[include SENSOR_BRIDGE "./dcm_walking/common/sensorBridge.ini"]

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:
 
         /**