diff --git a/CHANGELOG.md b/CHANGELOG.md
index 29585a54..c9dda980 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -3,9 +3,11 @@ All notable changes to this project are documented in this file.
 
 ## [Unreleased]
 ### Added
+- Add the possibility to use the walking-module with iCubGenova09 (https://github.com/robotology/walking-controllers/pull/80)
 
 ### Changed
 - Fixed missing link library in `WholeBodyControllers` component  (https://github.com/robotology/walking-controllers/pull/81).
+- Add the possibility to handle multiple wrenches in the RobotInterface/Helper class (https://github.com/robotology/walking-controllers/pull/80)
 
 ## [0.4.1] - 2020-02-04
 
@@ -121,7 +123,8 @@ All notable changes to this project are documented in this file.
 - Implement the first version of the `WalkingLoggerModule`
 - Implement the first version of the `WalkingJoypadModule`
 
-[Unreleased]: https://github.com/robotology/walking-controllers/compare/v0.4.0...devel
+[Unreleased]: https://github.com/robotology/walking-controllers/compare/v0.4.1...devel
+[0.4.1]: https://github.com/robotology/walking-controllers/compare/v0.4.0...v0.4.1
 [0.4.0]: https://github.com/robotology/walking-controllers/compare/v0.3.3...v0.4.0
 [0.3.3]: https://github.com/robotology/walking-controllers/compare/v0.3.2...v0.3.3
 [0.3.2]: https://github.com/robotology/walking-controllers/compare/v0.3.1...v0.3.2
diff --git a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h
index 57f5fd8d..ece43cd5 100644
--- a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h
+++ b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h
@@ -77,17 +77,21 @@ namespace WalkingControllers
         std::unique_ptr<iCub::ctrl::FirstOrderLowPassFilter> m_velocityFilter; /**< Joint velocity low pass filter .*/
         bool m_useVelocityFilter; /**< True if the joint velocity filter is used. */
 
-        yarp::os::BufferedPort<yarp::sig::Vector> m_leftWrenchPort; /**< Left foot wrench port. */
-        yarp::os::BufferedPort<yarp::sig::Vector> m_rightWrenchPort; /**< Right foot wrench port. */
-        yarp::sig::Vector m_leftWrenchInput; /**< YARP vector that contains left foot wrench. */
-        yarp::sig::Vector m_rightWrenchInput; /**< YARP vector that contains right foot wrench. */
-        yarp::sig::Vector m_leftWrenchInputFiltered; /**< YARP vector that contains left foot filtered wrench. */
-        yarp::sig::Vector m_rightWrenchInputFiltered; /**< YARP vector that contains right foot filtered wrench. */
+        struct MeasuredWrench
+        {
+            std::unique_ptr<yarp::os::BufferedPort<yarp::sig::Vector>> port; /**< yarp port. */
+            yarp::sig::Vector wrenchInput; /**< YARP vector that contains foot wrench. */
+            yarp::sig::Vector wrenchInputFiltered; /**< YARP vector that contains foot filtered wrench. */
+            std::unique_ptr<iCub::ctrl::FirstOrderLowPassFilter> lowPassFilter; /**< Low pass filter.*/
+            bool useFilter;
+            bool isUpdated;
+        };
+
+        std::vector<MeasuredWrench> m_leftFootMeasuredWrench;
+        std::vector<MeasuredWrench> m_rightFootMeasuredWrench;
+
         iDynTree::Wrench m_leftWrench; /**< iDynTree vector that contains left foot wrench. */
         iDynTree::Wrench m_rightWrench; /**< iDynTree vector that contains right foot wrench. */
-        std::unique_ptr<iCub::ctrl::FirstOrderLowPassFilter> m_leftWrenchFilter; /**< Left wrench low pass filter.*/
-        std::unique_ptr<iCub::ctrl::FirstOrderLowPassFilter> m_rightWrenchFilter; /**< Right wrench low pass filter.*/
-        bool m_useWrenchFilter; /**< True if the wrench filter is used. */
 
         double m_startingPositionControlTime;
         bool m_positionMoveSkipped;
@@ -120,6 +124,14 @@ namespace WalkingControllers
         bool setInteractionMode(yarp::dev::InteractionModeEnum interactionMode);
 
         bool setInteractionMode(std::vector<yarp::dev::InteractionModeEnum>& interactionModes);
+
+        bool configureForceTorqueSensor(const std::string& portPrefix,
+                                        const std::string& portInputName,
+                                        const std::string& wholeBodyDynamicsPortName,
+                                        const double& samplingTime,
+                                        bool useWrenchFilter,
+                                        double cutFrequency,
+                                        MeasuredWrench& measuredWrench);
     public:
 
         /**
diff --git a/src/RobotInterface/src/Helper.cpp b/src/RobotInterface/src/Helper.cpp
index 6ce5030f..9846f646 100644
--- a/src/RobotInterface/src/Helper.cpp
+++ b/src/RobotInterface/src/Helper.cpp
@@ -1,5 +1,6 @@
 #include <iDynTree/Core/Utils.h>
 #include <iDynTree/Core/EigenHelpers.h>
+#include <yarp/eigen/Eigen.h>
 #include <iDynTree/yarp/YARPConversions.h>
 
 #include <WalkingControllers/RobotInterface/Helper.h>
@@ -57,8 +58,15 @@ bool RobotInterface::getFeedbacksRaw(unsigned int maxAttempts)
     bool okPosition = false;
     bool okVelocity = false;
 
-    bool okLeftWrench = false;
-    bool okRightWrench = false;
+    // reset the measured wrenches
+    for(auto& wrench : m_leftFootMeasuredWrench)
+        wrench.isUpdated = false;
+
+    for(auto& wrench : m_rightFootMeasuredWrench)
+        wrench.isUpdated = false;
+
+    bool okLeftWrenches = false;
+    bool okRightWrenches = false;
 
     bool okBaseEstimation = !m_useExternalRobotBase;
 
@@ -71,25 +79,31 @@ bool RobotInterface::getFeedbacksRaw(unsigned int maxAttempts)
         if(!okVelocity)
             okVelocity = m_encodersInterface->getEncoderSpeeds(m_velocityFeedbackDeg.data());
 
-        if(!okLeftWrench)
+        for(auto& wrench : m_leftFootMeasuredWrench)
         {
-            yarp::sig::Vector *leftWrenchRaw = NULL;
-            leftWrenchRaw = m_leftWrenchPort.read(false);
-            if(leftWrenchRaw != NULL)
+            if(!wrench.isUpdated)
             {
-                m_leftWrenchInput = *leftWrenchRaw;
-                okLeftWrench = true;
+                yarp::sig::Vector *wrenchRaw = NULL;
+                wrenchRaw = wrench.port->read(false);
+                if(wrenchRaw != NULL)
+                {
+                    wrench.wrenchInput = *wrenchRaw;
+                    wrench.isUpdated = true;
+                }
             }
         }
 
-        if(!okRightWrench)
+        for(auto& wrench : m_rightFootMeasuredWrench)
         {
-            yarp::sig::Vector *rightWrenchRaw = NULL;
-            rightWrenchRaw = m_rightWrenchPort.read(false);
-            if(rightWrenchRaw != NULL)
+            if(!wrench.isUpdated)
             {
-                m_rightWrenchInput = *rightWrenchRaw;
-                okRightWrench = true;
+                yarp::sig::Vector *wrenchRaw = NULL;
+                wrenchRaw = wrench.port->read(false);
+                if(wrenchRaw != NULL)
+                {
+                    wrench.wrenchInput = *wrenchRaw;
+                    wrench.isUpdated = true;
+                }
             }
         }
 
@@ -113,7 +127,18 @@ bool RobotInterface::getFeedbacksRaw(unsigned int maxAttempts)
             }
         }
 
-        if(okPosition && okVelocity && okLeftWrench && okRightWrench && okBaseEstimation)
+
+        bool okLeft = true;
+        for(const auto& wrench : m_leftFootMeasuredWrench)
+            okLeft = okLeft && wrench.isUpdated;
+        okLeftWrenches = okLeft;
+
+        bool okRight = true;
+        for(const auto& wrench : m_rightFootMeasuredWrench)
+            okRight = okRight && wrench.isUpdated;
+        okRightWrenches = okRight;
+
+        if(okPosition && okVelocity && okLeftWrenches && okRightWrenches && okBaseEstimation)
         {
             for(unsigned j = 0 ; j < m_actuatedDOFs; j++)
             {
@@ -121,16 +146,45 @@ bool RobotInterface::getFeedbacksRaw(unsigned int maxAttempts)
                 m_velocityFeedbackRad(j) = iDynTree::deg2rad(m_velocityFeedbackDeg(j));
             }
 
-            if(!iDynTree::toiDynTree(m_leftWrenchInput, m_leftWrench))
+            // let's sum all the wrenches together. Notice here we assume that
+            // the wrenches on the foot are computed with the same pole.
+
+            // left foot
+            // copy the first wrench
+            auto leftWrenchIt = m_leftFootMeasuredWrench.begin();
+            if(!iDynTree::toiDynTree(leftWrenchIt->wrenchInput, m_leftWrench))
             {
                 yError() << "[RobotInterface::getFeedbacksRaw] Unable to convert left foot wrench.";
                 return false;
             }
-            if(!iDynTree::toiDynTree(m_rightWrenchInput, m_rightWrench))
+
+            // sum all the other wrenches
+            std::advance(leftWrenchIt, 1);
+            for(;leftWrenchIt != m_leftFootMeasuredWrench.end(); std::advance(leftWrenchIt, 1))
+            {
+                // the idyntree wrench raw data  is not a consecutive array
+                iDynTree::toEigen(m_leftWrench.getLinearVec3()) = yarp::eigen::toEigen(leftWrenchIt->wrenchInput).head<3>();
+                iDynTree::toEigen(m_leftWrench.getAngularVec3()) = yarp::eigen::toEigen(leftWrenchIt->wrenchInput).tail<3>();
+            }
+
+            // right foot
+            // copy the first wrench
+            auto rightWrenchIt = m_rightFootMeasuredWrench.begin();
+            if(!iDynTree::toiDynTree(rightWrenchIt->wrenchInput, m_rightWrench))
             {
                 yError() << "[RobotInterface::getFeedbacksRaw] Unable to convert right foot wrench.";
                 return false;
             }
+
+            // sum all the other wrenches
+            std::advance(rightWrenchIt, 1);
+            for(;rightWrenchIt != m_rightFootMeasuredWrench.end(); std::advance(rightWrenchIt, 1))
+            {
+                // the idyntree wrench raw data  is not a consecutive array
+                iDynTree::toEigen(m_rightWrench.getLinearVec3()) = yarp::eigen::toEigen(rightWrenchIt->wrenchInput).head<3>();
+                iDynTree::toEigen(m_rightWrench.getAngularVec3()) = yarp::eigen::toEigen(rightWrenchIt->wrenchInput).tail<3>();
+            }
+
             return true;
         }
         yarp::os::Time::delay(0.001);
@@ -144,11 +198,11 @@ bool RobotInterface::getFeedbacksRaw(unsigned int maxAttempts)
     if(!okVelocity)
         yError() << "\t - Velocity encoders";
 
-    if(!okLeftWrench)
-        yError() << "\t - Left wrench";
+    if(!okLeftWrenches)
+        yError() << "\t - Left wrenches";
 
-    if(!okRightWrench)
-        yError() << "\t - Right wrench";
+    if(!okRightWrenches)
+        yError() << "\t - Right wrenches";
 
     if(!okBaseEstimation)
         yError() << "\t - Base estimation";
@@ -428,6 +482,33 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config)
     return true;
 }
 
+bool RobotInterface::configureForceTorqueSensor(const std::string& portPrefix,
+                                                const std::string& portInputName,
+                                                const std::string& wholeBodyDynamicsPortName,
+                                                const double& samplingTime,
+                                                bool useWrenchFilter,
+                                                double cutFrequency,
+                                                MeasuredWrench& measuredWrench)
+{
+
+    measuredWrench.port = std::make_unique<yarp::os::BufferedPort<yarp::sig::Vector>>();
+    measuredWrench.port->open("/" + portPrefix + portInputName);
+    // connect port
+    if(!yarp::os::Network::connect(wholeBodyDynamicsPortName, "/" + portPrefix + portInputName))
+    {
+        yError() << "[RobotInterface::configureForceTorqueSensors] Unable to connect to port "
+                 << wholeBodyDynamicsPortName << " to " << "/" + portPrefix + portInputName;
+        return false;
+    }
+
+    if(useWrenchFilter)
+    {
+        measuredWrench.lowPassFilter = std::make_unique<iCub::ctrl::FirstOrderLowPassFilter>(cutFrequency,
+                                                                                             samplingTime);
+    }
+    return true;
+}
+
 bool RobotInterface::configureForceTorqueSensors(const yarp::os::Searchable& config)
 {
     std::string portInput, portOutput;
@@ -439,74 +520,142 @@ bool RobotInterface::configureForceTorqueSensors(const yarp::os::Searchable& con
         return false;
     }
 
-    std::string name;
-    if(!YarpUtilities::getStringFromSearchable(config, "name", name))
+    std::string portPrefix;
+    if(!YarpUtilities::getStringFromSearchable(config, "name", portPrefix))
     {
         yError() << "[RobotInterface::configureForceTorqueSensors] Unable to get the string from searchable.";
         return false;
     }
 
-    double sampligTime = config.check("sampling_time", yarp::os::Value(0.016)).asDouble();
+    double samplingTime = config.check("sampling_time", yarp::os::Value(0.016)).asDouble();
 
-    // open and connect left foot wrench
-    if(!YarpUtilities::getStringFromSearchable(config, "leftFootWrenchInputPort_name", portInput))
+    // collect the information for the ports of the left foot wrenches
+    // from now on we assume that the wrenches are expressed in the same frame (e.g l_sole)
+    yarp::os::Value* listLeftFootWrenchInputPorts;
+    std::string key = "left_foot_wrench_input_port_name";
+    if(!config.check(key, listLeftFootWrenchInputPorts))
     {
-        yError() << "[RobotInterface::configureForceTorqueSensors] Unable to get "
-            "the string from searchable.";
+        yError() << "[RobotInterface::configureForceTorqueSensors] Missing field "<< key;
         return false;
     }
-    if(!YarpUtilities::getStringFromSearchable(config, "leftFootWrenchOutputPort_name", portOutput))
+    std::vector<std::string> leftFootWrenchInputPorts;
+    if (!YarpUtilities::yarpListToStringVector(listLeftFootWrenchInputPorts,
+                                               leftFootWrenchInputPorts))
     {
-        yError() << "[RobotInterface::configureForceTorqueSensors] Unable to get the string from searchable.";
+      yError() << "[RobotInterface::configureForceTorqueSensors] Unable to "
+                  "convert the yarp list into a vector of string.";
+      return false;
+    }
+
+    yarp::os::Value* listLeftFootWrenchOutputPorts;
+    key = "left_foot_wrench_output_port_name";
+    if(!config.check(key, listLeftFootWrenchOutputPorts))
+    {
+        yError() << "[RobotInterface::configureForceTorqueSensors] Missing field "<< key;
         return false;
     }
-    // open port
-    m_leftWrenchPort.open("/" + name + portInput);
-    // connect port
-    if(!yarp::os::Network::connect(portOutput, "/" + name + portInput))
+    std::vector<std::string> leftFootWrenchOutputPorts;
+    if (!YarpUtilities::yarpListToStringVector(listLeftFootWrenchOutputPorts,
+                                               leftFootWrenchOutputPorts))
     {
-        yError() << "[RobotInterface::configureForceTorqueSensors] Unable to connect to port "
-                 << portOutput << " to " << "/" + name + portInput;
+        yError() << "[RobotInterface::configureForceTorqueSensors] Unable to "
+                    "convert the yarp list into a vector of string.";
         return false;
     }
 
-    // open and connect right foot wrench
-    if(!YarpUtilities::getStringFromSearchable(config, "rightFootWrenchInputPort_name", portInput))
+    if (leftFootWrenchOutputPorts.size() != leftFootWrenchInputPorts.size())
     {
-        yError() << "[RobotInterface::configureForceTorqueSensors] Unable to get the string from searchable.";
+        yError() << "[RobotInterface::configureForceTorqueSensors] The number of "
+                    "the input port associated to the left foot and the output port should be the "
+                    "same. The input ports are "
+                 << leftFootWrenchInputPorts.size() << ", while the output ports "
+                 << leftFootWrenchOutputPorts.size();
         return false;
     }
-    if(!YarpUtilities::getStringFromSearchable(config, "rightFootWrenchOutputPort_name", portOutput))
+
+
+    // collect the information for the ports of the right foot wrenches
+    // from now on we assume that the wrenches are expressed in the same frame (e.g l_sole)
+    yarp::os::Value* listRightFootWrenchInputPorts;
+    key = "right_foot_wrench_input_port_name";
+    if(!config.check(key, listRightFootWrenchInputPorts))
     {
-        yError() << "[RobotInterface::configureForceTorqueSensors] Unable to get the string from searchable.";
+        yError() << "[RobotInterface::configureForceTorqueSensors] Missing field "<< key;
         return false;
     }
-    // open port
-    m_rightWrenchPort.open("/" + name + portInput);
-    // connect port
-    if(!yarp::os::Network::connect(portOutput, "/" + name + portInput))
+    std::vector<std::string> rightFootWrenchInputPorts;
+    if (!YarpUtilities::yarpListToStringVector(listRightFootWrenchInputPorts,
+                                               rightFootWrenchInputPorts))
     {
-        yError() << "[RobotInterface::configureForceTorqueSensors] Unable to connect to port "
-                 << portOutput << " to " << "/" + name + portInput;
+        yError() << "[RobotInterface::configureForceTorqueSensors] Unable to "
+            "convert the yarp list into a vector of string.";
         return false;
     }
 
-    m_useWrenchFilter = config.check("use_wrench_filter", yarp::os::Value("False")).asBool();
-    if(m_useWrenchFilter)
+    yarp::os::Value* listRightFootWrenchOutputPorts;
+    key = "right_foot_wrench_output_port_name";
+    if(!config.check(key, listRightFootWrenchOutputPorts))
+    {
+        yError() << "[RobotInterface::configureForceTorqueSensors] Missing field "<< key;
+        return false;
+    }
+    std::vector<std::string> rightFootWrenchOutputPorts;
+    if (!YarpUtilities::yarpListToStringVector(listRightFootWrenchOutputPorts,
+                                               rightFootWrenchOutputPorts))
+    {
+        yError() << "[RobotInterface::configureForceTorqueSensors] Unable to "
+            "convert the yarp list into a vector of string.";
+        return false;
+    }
+
+    if (rightFootWrenchOutputPorts.size() != rightFootWrenchInputPorts.size())
+    {
+        yError() << "[RobotInterface::configureForceTorqueSensors] The number of "
+            "the input port associated to the right foot and the output port should be the "
+            "same. The input ports are "
+                 << rightFootWrenchInputPorts.size() << ", while the output ports "
+                 << rightFootWrenchOutputPorts.size();
+        return false;
+    }
+
+    // resize the wrenches vectors
+    m_leftFootMeasuredWrench.resize(leftFootWrenchOutputPorts.size());
+    m_rightFootMeasuredWrench.resize(rightFootWrenchOutputPorts.size());
+
+    const bool useWrenchFilter = config.check("use_wrench_filter", yarp::os::Value("False")).asBool();
+    double cutFrequency = 0;
+    if(useWrenchFilter)
     {
-        double cutFrequency;
         if(!YarpUtilities::getNumberFromSearchable(config, "wrench_cut_frequency", cutFrequency))
         {
             yError() << "[RobotInterface::configureForceTorqueSensors] Unable get double from searchable.";
             return false;
         }
+    }
 
-        m_leftWrenchFilter = std::make_unique<iCub::ctrl::FirstOrderLowPassFilter>(cutFrequency,
-                                                                                   sampligTime);
-        m_rightWrenchFilter = std::make_unique<iCub::ctrl::FirstOrderLowPassFilter>(cutFrequency,
-                                                                                    sampligTime);
+    // initialize the measured wrenches
+    bool ok = true;
+    for (std::size_t i = 0; i < leftFootWrenchOutputPorts.size(); i++)
+    {
+        ok = ok && this->configureForceTorqueSensor(portPrefix,
+                                                    leftFootWrenchInputPorts[i],
+                                                    leftFootWrenchOutputPorts[i],
+                                                    samplingTime,
+                                                    useWrenchFilter, cutFrequency,
+                                                    m_leftFootMeasuredWrench[i]);
     }
-    return true;
+
+    for (std::size_t i = 0; i < rightFootWrenchOutputPorts.size(); i++)
+    {
+        ok = ok && this->configureForceTorqueSensor(portPrefix,
+                                                    rightFootWrenchInputPorts[i],
+                                                    rightFootWrenchOutputPorts[i],
+                                                    samplingTime,
+                                                    useWrenchFilter, cutFrequency,
+                                                    m_rightFootMeasuredWrench[i]);
+    }
+
+    return ok;
 }
 
 bool RobotInterface::configurePIDHandler(const yarp::os::Bottle& config)
@@ -527,12 +676,37 @@ bool RobotInterface::resetFilters()
     if(m_useVelocityFilter)
         m_velocityFilter->init(m_velocityFeedbackDeg);
 
-    if(m_useWrenchFilter)
+    for(auto& wrench : m_leftFootMeasuredWrench)
     {
-        m_leftWrenchFilter->init(m_leftWrenchInput);
-        m_rightWrenchFilter->init(m_rightWrenchInput);
+        if(wrench.useFilter)
+        {
+            if(!wrench.isUpdated)
+            {
+                yError() << "[RobotInterface::resetFilters] The left wrench is not updated. I cannot reset the filter.";
+                return false;
+            }
+
+            // reset the filter
+            wrench.lowPassFilter->init(wrench.wrenchInput);
+        }
+    }
+
+    for(auto& wrench : m_rightFootMeasuredWrench)
+    {
+        if(wrench.useFilter)
+        {
+            if(!wrench.isUpdated)
+            {
+                yError() << "[RobotInterface::resetFilters] The right wrench is not updated. I cannot reset the filter.";
+                return false;
+            }
+
+            // reset the filter
+            wrench.lowPassFilter->init(wrench.wrenchInput);
+        }
     }
 
+
     return true;
 }
 
@@ -551,22 +725,99 @@ bool RobotInterface::getFeedbacks(unsigned int maxAttempts)
         for(unsigned j = 0; j < m_actuatedDOFs; ++j)
             m_velocityFeedbackRad(j) = iDynTree::deg2rad(m_velocityFeedbackDegFiltered(j));
     }
-    if(m_useWrenchFilter)
+
+    // filt all the wrenches
+    for(auto& wrench : m_leftFootMeasuredWrench)
     {
-        m_leftWrenchInputFiltered = m_leftWrenchFilter->filt(m_leftWrenchInput);
-        m_rightWrenchInputFiltered = m_rightWrenchFilter->filt(m_rightWrenchInput);
+        if(wrench.useFilter)
+        {
+            if(!wrench.isUpdated)
+            {
+                yError() << "[RobotInterface::getFeedbacks] The left wrench is not updated. I cannot filter it.";
+                return false;
+            }
 
-        if(!iDynTree::toiDynTree(m_leftWrenchInputFiltered, m_leftWrench))
+            // apply the filter
+            wrench.wrenchInputFiltered =  wrench.lowPassFilter->filt(wrench.wrenchInput);
+        }
+    }
+
+    for(auto& wrench : m_rightFootMeasuredWrench)
+    {
+        if(wrench.useFilter)
         {
-            yError() << "[RobotInterface::getFeedbacks] Unable to convert left foot wrench.";
-            return false;
+            if(!wrench.isUpdated)
+            {
+                yError() << "[RobotInterface::getFeedbacks] The right wrench is not updated. I cannot filter it.";
+                return false;
+            }
+
+            // apply the filter
+            wrench.wrenchInputFiltered =  wrench.lowPassFilter->filt(wrench.wrenchInput);
         }
-        if(!iDynTree::toiDynTree(m_rightWrenchInputFiltered, m_rightWrench))
+    }
+
+    // left foot
+    // copy the first wrench
+    auto leftWrenchIt = m_leftFootMeasuredWrench.begin();
+    const yarp::sig::Vector* leftWrenchVectorPtr = &(leftWrenchIt->wrenchInput);
+    if(leftWrenchIt->useFilter)
+    {
+        leftWrenchVectorPtr = &(leftWrenchIt->wrenchInputFiltered);
+    }
+
+    if(!iDynTree::toiDynTree(*leftWrenchVectorPtr, m_leftWrench))
+    {
+        yError() << "[RobotInterface::getFeedbacks] Unable to convert left foot wrench.";
+        return false;
+    }
+
+    // sum all the other wrenches
+    std::advance(leftWrenchIt, 1);
+    for(;leftWrenchIt != m_leftFootMeasuredWrench.end(); std::advance(leftWrenchIt, 1))
+    {
+        yarp::sig::Vector* leftWrenchVectorPtr = &(leftWrenchIt->wrenchInput);
+        if(leftWrenchIt->useFilter)
         {
-            yError() << "[RobotInterface::getFeedbacks] Unable to convert right foot wrench.";
-            return false;
+            leftWrenchVectorPtr = &(leftWrenchIt->wrenchInputFiltered);
+        }
+
+        // the idyntree wrench raw data  is not a consecutive array
+        iDynTree::toEigen(m_leftWrench.getLinearVec3()) = yarp::eigen::toEigen(*leftWrenchVectorPtr).head<3>();
+        iDynTree::toEigen(m_leftWrench.getAngularVec3()) = yarp::eigen::toEigen(*leftWrenchVectorPtr).tail<3>();
+    }
+
+    // right foot
+    // copy the first wrench
+    auto rightWrenchIt = m_rightFootMeasuredWrench.begin();
+    const yarp::sig::Vector* rightWrenchVectorPtr = &(rightWrenchIt->wrenchInput);
+    if(rightWrenchIt->useFilter)
+    {
+        rightWrenchVectorPtr = &(rightWrenchIt->wrenchInputFiltered);
+    }
+
+    if(!iDynTree::toiDynTree(*rightWrenchVectorPtr, m_rightWrench))
+    {
+        yError() << "[RobotInterface::getFeedbacks] Unable to convert right foot wrench.";
+        return false;
+    }
+
+    // sum all the other wrenches
+    std::advance(rightWrenchIt, 1);
+    for(;rightWrenchIt != m_rightFootMeasuredWrench.end(); std::advance(rightWrenchIt, 1))
+    {
+        yarp::sig::Vector* rightWrenchVectorPtr = &(rightWrenchIt->wrenchInput);
+        if(rightWrenchIt->useFilter)
+        {
+            rightWrenchVectorPtr = &(rightWrenchIt->wrenchInputFiltered);
         }
+
+        // the idyntree wrench raw data  is not a consecutive array
+        iDynTree::toEigen(m_rightWrench.getLinearVec3()) = yarp::eigen::toEigen(*rightWrenchVectorPtr).head<3>();
+        iDynTree::toEigen(m_rightWrench.getAngularVec3()) = yarp::eigen::toEigen(*rightWrenchVectorPtr).tail<3>();
     }
+
+
     return true;
 }
 
@@ -834,8 +1085,13 @@ bool RobotInterface::setVelocityReferences(const iDynTree::VectorDynSize& desire
 
 bool RobotInterface::close()
 {
-    m_rightWrenchPort.close();
-    m_leftWrenchPort.close();
+    // close all the ports
+    for(auto& wrench : m_leftFootMeasuredWrench)
+        wrench.port->close();
+
+    for(auto& wrench : m_rightFootMeasuredWrench)
+        wrench.port->close();
+
     switchToControlMode(VOCAB_CM_POSITION);
     m_controlMode = VOCAB_CM_POSITION;
     setInteractionMode(yarp::dev::InteractionModeEnum::VOCAB_IM_STIFF);
diff --git a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp
index 95221c5b..13724040 100644
--- a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp
+++ b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp
@@ -129,7 +129,7 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config)
     ok = ok && unicyclePlanner->setMinimumAngleForNewSteps(minAngleVariation);
     ok = ok && unicyclePlanner->setMinimumStepLength(minStepLength);
     ok = ok && unicyclePlanner->setSlowWhenTurnGain(slowWhenTurningGain);
-    unicyclePlanner->addTerminalStep(false);
+    unicyclePlanner->addTerminalStep(true);
     unicyclePlanner->startWithLeft(m_swingLeft);
     unicyclePlanner->resetStartingFootIfStill(startWithSameFoot);
 
diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/common/forceTorqueSensors.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/common/forceTorqueSensors.ini
index 3326b621..5590bb48 100644
--- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/common/forceTorqueSensors.ini
+++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/common/forceTorqueSensors.ini
@@ -1,6 +1,6 @@
 # name of the left and right feet wrench ports
-leftFootWrenchInputPort_name     /leftFootWrench:i
-rightFootWrenchInputPort_name    /rightFootWrench:i
+left_foot_wrench_input_port_name     ("/left_foot/cartesianEndEffectorWrench:i")
+left_foot_wrench_output_port_name    ("/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o")
 
-leftFootWrenchOutputPort_name     /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o
-rightFootWrenchOutputPort_name    /wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o
+right_foot_wrench_input_port_name    ("/right_foot/cartesianEndEffectorWrench:i")
+right_foot_wrench_output_port_name   ("/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o")
diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/controllerParams.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/controllerParams.ini
new file mode 100644
index 00000000..4ec0ab0c
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/controllerParams.ini
@@ -0,0 +1,10 @@
+controllerHorizon       2
+
+stateWeightTriplets     ((0,0,7500), (1,1,7500))
+inputWeightTriplets     ((0,0,9000000), (1,1,9000000))
+
+#Foot Dimensions          x_min   x_max  y_min   y_max
+foot_size               ((-0.02   0.05), (-0.025   0.025))
+initial_zmp_position    (0.0 0.0)
+
+convex_hull_tolerance   0.05
diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/dcmReactiveControllerParams.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/dcmReactiveControllerParams.ini
new file mode 100644
index 00000000..0e1b4a67
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/dcmReactiveControllerParams.ini
@@ -0,0 +1 @@
+kDCM                    1.2
diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/forceTorqueSensors.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/forceTorqueSensors.ini
new file mode 100644
index 00000000..6916c035
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/forceTorqueSensors.ini
@@ -0,0 +1,10 @@
+# name of the left and right feet wrench ports
+left_foot_wrench_input_port_name     ("/left_foot_front/cartesianEndEffectorWrench:i",
+                                      "/left_foot_rear/cartesianEndEffectorWrench:i")
+left_foot_wrench_output_port_name    ("/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o",
+                                      "/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o")
+
+right_foot_wrench_input_port_name     ("/right_foot_front/cartesianEndEffectorWrench:i",
+                                      "/right_foot_rear/cartesianEndEffectorWrench:i")
+right_foot_wrench_output_port_name    ("/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o",
+                                      "/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o")
diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/forwardKinematics.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/forwardKinematics.ini
new file mode 100644
index 00000000..89f643ca
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/forwardKinematics.ini
@@ -0,0 +1,18 @@
+#the x axis pointing forward the z axis pointing upward and the y concludes a right-handed frame
+left_foot_frame         l_sole
+right_foot_frame        r_sole
+
+# hand frames
+left_hand_frame         l_sole
+right_hand_frame        r_sole
+
+head_frame              l_sole
+
+root_frame              root_link
+torso_frame             neck_2
+
+# filters
+# if it is equal to 0 the low pass filters are not used
+use_filters             0
+                        #Hz
+cut_frequency           10.0
diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/pidParams.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/pidParams.ini
new file mode 100644
index 00000000..33b08ee6
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/pidParams.ini
@@ -0,0 +1,27 @@
+# PID handling file
+
+# All the following phases are those of the left foot. In a specific phase, a joint will have the PIDs specified in the corresponding section. If not, the default will be used (if specified, the original PID otherwise).
+
+# Possible phases: SWING_LEFT, SWING_RIGHT, SWITCH
+
+# Each group, except DEFAULT, contains:
+#    -the field activationPhase with one of the possible phases defined above,
+#    -the field activationOffset which specified the time advance/delay (if the sign is + it will be a delay) wrt the begin of the phase (OPTIONAL, default 0.0)
+#    -the field smoothingTime which defines the smoothing time with which the PIDs will be changed (OPTIONAL, default 1.0)
+
+# if 0 only the default group will be taken into consideration
+useGainScheduling           0
+
+[DEFAULT]
+#NAME                P     I      D
+#torso_pitch         5.0   1.0    0.5
+#l_knee             30.0  0.30   0.30
+#r_knee             30.0  0.30   0.30
+#l_ankle_pitch      35.0  0.35   0.35
+#r_ankle_pitch      35.0  0.35   0.35
+#l_ankle_roll       35.0  0.35   0.35
+#r_ankle_roll       35.0  0.35   0.35
+#l_hip_pitch        35.0  0.35   0.35
+#r_hip_pitch        35.0  0.35   0.35
+#l_hip_roll         35.0  0.35   0.35
+#r_hip_roll         35.0  0.35   0.35
diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/plannerParams.ini
new file mode 100644
index 00000000..52bf4eb5
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/plannerParams.ini
@@ -0,0 +1,59 @@
+##Timings
+plannerHorizon          10.0
+
+##Unicycle Related Quantities
+unicycleGain            10.0
+referencePosition       (0.1 0.0)
+timeWeight              2.5
+positionWeight          1.0
+slowWhenTurningGain     5.0
+
+##Bounds
+#Step length
+maxStepLength           0.22
+minStepLength           0.05
+#Width
+minWidth                0.18
+#Angle Variations in DEGREES
+#maxAngleVariation       12.0
+maxAngleVariation       10.0
+minAngleVariation       8.0
+#Timings
+maxStepDuration         5.0
+minStepDuration         4.0
+
+##Nominal Values
+#Width
+nominalWidth            0.20
+#Height
+stepHeight              0.03
+stepLandingVelocity     0.0
+footApexTime            0.8
+comHeightDelta          0.01
+#Timings
+nominalDuration         4.9
+lastStepSwitchTime      0.8
+switchOverSwingRatio    0.7
+
+#ZMP Delta
+leftZMPDelta           (0.0 -0.0)
+rightZMPDelta          (0.0 0.0)
+
+# Last Step DCM Offset
+# If it is 0.5 the final DCM will be in the middle of the two footsteps;
+# If it is 0 the DCM position coincides with the stance foot ZMP;
+# If it is 1 the DCM position coincides with the next foot ZMP position.
+lastStepDCMOffset      0.25
+
+#MergePoint
+mergePointRatio         0.4
+
+# pitch delta
+pitchDelta		0.0
+
+##Should be the first step with the left foot?
+swingLeft               1
+startAlwaysSameFoot     1
+
+##Remove this line if you don't want to use the minimum jerk trajectory in feet interpolation
+# useMinimumJerkFootTrajectory    1
diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/walkingLogger.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/walkingLogger.ini
new file mode 100644
index 00000000..943ae735
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/walkingLogger.ini
@@ -0,0 +1,5 @@
+dataLoggerOutputPort_name         /logger/data:o
+dataLoggerRpcOutputPort_name      /logger/rpc:o
+
+dataLoggerInputPort_name          /logger/data:i
+dataLoggerRpcInputPort_name       /logger/rpc:i
diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/zmpControllerParams.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/zmpControllerParams.ini
new file mode 100644
index 00000000..c814e3e5
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/zmpControllerParams.ini
@@ -0,0 +1,20 @@
+# remove the following line if you do not want the gain scheduling
+useGainScheduling      1
+
+smoothingTime          0.05
+
+# if the gain scheduling is off only k*_walking parameters are used
+kZMP_walking           0.0
+# kZMP_stance            1.0
+kCoM_walking           5.0
+
+kZMP_stance            0.0
+# kZMP_stance            1.0
+kCoM_stance            5.0
+
+# old parameters
+#kZMP                    2.0 low velocity reactive
+#kZMP                    1.9 low velocity MPC
+
+#kCoM                    10.0 low velocity reactive
+#kCoM                    8.0 low velocity MPC
diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/handRetargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/handRetargeting.ini
new file mode 100644
index 00000000..8c9da41c
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/handRetargeting.ini
@@ -0,0 +1,2 @@
+# This file is empty because when the robot is controller using only the joypad
+# the retargeting is disabled
diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/inverseKinematics.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/inverseKinematics.ini
new file mode 100644
index 00000000..9a253b62
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/inverseKinematics.ini
@@ -0,0 +1,23 @@
+#The left_foot_frame is supposed to have the same orientation of the Inertia frame, which have the z axis pointing upwards,
+#the x axis pointing forward and and the y concludes a right-handed frame
+left_foot_frame         l_sole
+right_foot_frame        r_sole
+
+# #Remove the following line in order not to consider the
+# #additional frame
+additional_frame        chest
+
+# #The additional rotation is defined (by rows) in the Inertia frame.
+#Remove the following line to keep the identity as additional rotation
+additional_rotation     ((1.0 0.0 0.0),(0.0 1.0 0.0),(0.0 0.0 1.0))
+
+# solver paramenters
+solver-verbosity        0
+solver_name             ma27
+max-cpu-time            20
+
+#DEGREES
+jointRegularization     (15, 0, 0, -2, 22, 11, 30, -2, 22, 11, 30, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351)
+
+#jointRegularization     (0, 15, 0, 0, -29.794, 29.97, 0.00, 44.977, -29.794, 29.97, 0.00, 44.977, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351)
+#jointRegularization     (15, 0, 0, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351)
diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/qpInverseKinematics.ini
new file mode 100644
index 00000000..54c8f81f
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/qpInverseKinematics.ini
@@ -0,0 +1,38 @@
+# CoM
+use_com_as_constraint           1
+# comWeightTriplets              ((0,0,100), (1,1,100), (2,2,100))
+
+# weight matrices related to the neck
+neck_weight                     5.0
+additional_rotation             ((1.0 0.0 0.0),(0.0 1.0 0.0),(0.0 0.0 1.0))
+
+# Joint regularization term
+joint_regularization            (15, 0, 0,
+                               -7, 22, 11, 30,
+                               -7, 22, 11, 30,
+                               5.082, 0.406, -0.131, -45.249, -26.454, -0.351,
+                               5.082, 0.406, -0.131, -45.249, -26.454, -0.351)
+
+joint_regularization_weights     (1.0, 1.0, 1.0,
+                               2.0, 2.0, 2.0, 2.0,
+                               2.0, 2.0, 2.0, 2.0,
+                               1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                               1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+
+joint_regularization_gains     (5.0, 5.0, 5.0,
+                               5.0, 5.0, 5.0, 5.0,
+                               5.0, 5.0, 5.0, 5.0,
+                               5.0, 5.0, 5.0, 5.0, 5.0, 5.0,
+                               5.0, 5.0, 5.0, 5.0, 5.0, 5.0)
+
+# Gains
+k_posCom                        0.0
+k_posFoot                       10.0
+k_attFoot                       5.0
+k_neck                          1.0
+
+# joint limits
+k_joint_limit_lower_bound       1.0
+k_joint_limit_upper_bound       1.0
+
+# use_joint_limits_constraint   1
diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/robotControl.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/robotControl.ini
new file mode 100644
index 00000000..2c95edc9
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/robotControl.ini
@@ -0,0 +1,32 @@
+robot                   icubSim
+
+joints_list             ("torso_pitch", "torso_roll", "torso_yaw",
+                        "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow",
+                        "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow",
+                        "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll",
+                        "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")
+
+remote_control_boards   ("torso", "left_arm", "right_arm", "left_leg", "right_leg")
+
+# filters
+# if use_*_filter is equal to 0 the low pass filters are not used
+use_joint_velocity_filter          0
+joint_velocity_cut_frequency       10.0
+
+use_wrench_filter                  0
+wrench_cut_frequency               10.0
+
+
+# if true the joint is in stiff mode if false the joint is in compliant mode
+joint_is_stiff_mode     (true, true, true,
+                         true, true, true, true,
+                         true, true, true, true,
+                         true, true, true, true, true, true,
+                         true, true, true, true, true, true)
+
+# if true a good joint traking is considered mandatory
+good_tracking_required  (true, true, true,
+                         true, true, true, true,
+                         true, true, true, true,
+                         true, true, true, true, true, true,
+                         true, true, true, true, true, true)
diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_retargeting.ini
new file mode 100644
index 00000000..3247b591
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_retargeting.ini
@@ -0,0 +1,60 @@
+# Remove this line if you don't want to use the MPC
+# use_mpc                            1
+
+# Remove this line if you don't want to use the QP-IK
+use_QP-IK                          1
+
+# Remove this line if you don't want to use osqp to
+# solve QP-IK. In this case qpOASES will be used
+# use_osqp                           1
+
+# remove this line if you don't want to save data of the experiment
+# dump_data                          1
+
+# general parameters
+[GENERAL]
+name                    walking-coordinator
+# height of the com
+com_height              0.53
+# sampling time
+sampling_time           0.01
+# enable hand retargeting
+use_hand_retargeting    1
+# enable the virtualizer
+use_virtualizer         1
+
+# include robot control parameters
+[include ROBOT_CONTROL "./dcm_walking/hand_retargeting/robotControl.ini"]
+
+# include trajectory planner parameters
+[include TRAJECTORY_PLANNER "./dcm_walking/common/plannerParams.ini"]
+
+# include MPC parameters
+[include DCM_MPC_CONTROLLER "./dcm_walking/common/controllerParams.ini"]
+
+# include MPC parameters
+[include DCM_REACTIVE_CONTROLLER "./dcm_walking/common/dcmReactiveControllerParams.ini"]
+
+# include MPC parameters
+[include ZMP_CONTROLLER "./dcm_walking/common/zmpControllerParams.ini"]
+
+# include inverse kinematcs parameters
+[include INVERSE_KINEMATICS_SOLVER "./dcm_walking/hand_retargeting/inverseKinematics.ini"]
+
+# include qp inverse kinematcs parameters
+[include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/hand_retargeting/qpInverseKinematics.ini"]
+
+# include inverse kinematcs parameters
+[include FORWARD_KINEMATICS_SOLVER "./dcm_walking/common/forwardKinematics.ini"]
+
+# include FT sensors parameters
+[include FT_SENSORS "./dcm_walking/common/forceTorqueSensors.ini"]
+
+# include Logger parameters
+[include WALKING_LOGGER "./dcm_walking/common/walkingLogger.ini"]
+
+# include lower PID parameters
+[include PID "./dcm_walking/common/pidParams.ini"]
+
+# retargeting
+[include RETARGETING "./dcm_walking/hand_retargeting/handRetargeting.ini"]
diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/forceTorqueSensors.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/forceTorqueSensors.ini
index 3326b621..5590bb48 100644
--- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/forceTorqueSensors.ini
+++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/forceTorqueSensors.ini
@@ -1,6 +1,6 @@
 # name of the left and right feet wrench ports
-leftFootWrenchInputPort_name     /leftFootWrench:i
-rightFootWrenchInputPort_name    /rightFootWrench:i
+left_foot_wrench_input_port_name     ("/left_foot/cartesianEndEffectorWrench:i")
+left_foot_wrench_output_port_name    ("/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o")
 
-leftFootWrenchOutputPort_name     /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o
-rightFootWrenchOutputPort_name    /wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o
+right_foot_wrench_input_port_name    ("/right_foot/cartesianEndEffectorWrench:i")
+right_foot_wrench_output_port_name   ("/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o")
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/controllerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/controllerParams.ini
new file mode 100644
index 00000000..112d2aa8
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/controllerParams.ini
@@ -0,0 +1,10 @@
+controllerHorizon       2
+
+stateWeightTriplets     ((0,0,7500), (1,1,7500))
+inputWeightTriplets     ((0,0,9000000), (1,1,9000000))
+
+#Foot Dimensions          x_min   x_max  y_min   y_max
+foot_size               ((-0.12   0.12), (-0.05   0.05))
+initial_zmp_position    (0.0 0.0)
+
+convex_hull_tolerance   0.05
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/dcmReactiveControllerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/dcmReactiveControllerParams.ini
new file mode 100644
index 00000000..0e1b4a67
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/dcmReactiveControllerParams.ini
@@ -0,0 +1 @@
+kDCM                    1.2
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/forceTorqueSensors.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/forceTorqueSensors.ini
new file mode 100644
index 00000000..6916c035
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/forceTorqueSensors.ini
@@ -0,0 +1,10 @@
+# name of the left and right feet wrench ports
+left_foot_wrench_input_port_name     ("/left_foot_front/cartesianEndEffectorWrench:i",
+                                      "/left_foot_rear/cartesianEndEffectorWrench:i")
+left_foot_wrench_output_port_name    ("/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o",
+                                      "/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o")
+
+right_foot_wrench_input_port_name     ("/right_foot_front/cartesianEndEffectorWrench:i",
+                                      "/right_foot_rear/cartesianEndEffectorWrench:i")
+right_foot_wrench_output_port_name    ("/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o",
+                                      "/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o")
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/forwardKinematics.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/forwardKinematics.ini
new file mode 100644
index 00000000..aad467c6
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/forwardKinematics.ini
@@ -0,0 +1,18 @@
+#the x axis pointing forward the z axis pointing upward and the y concludes a right-handed frame
+left_foot_frame         l_sole
+right_foot_frame        r_sole
+
+# hand frames
+left_hand_frame         l_hand
+right_hand_frame        r_hand
+
+head_frame              head
+
+root_frame              root_link
+torso_frame             neck_2
+
+# filters
+# if it is equal to 0 the low pass filters are not used
+use_filters             0
+                        #Hz
+cut_frequency           10.0
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini
new file mode 100644
index 00000000..c87427e4
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini
@@ -0,0 +1,29 @@
+# PID handling file
+
+# All the following phases are those of the left foot. In a specific phase, a joint will have the PIDs specified in the corresponding section. If not, the default will be used (if specified, the original PID otherwise).
+
+# Possible phases: SWING_LEFT, SWING_RIGHT, SWITCH
+
+# Each group, except DEFAULT, contains:
+#    -the field activationPhase with one of the possible phases defined above,
+#    -the field activationOffset which specified the time advance/delay (if the sign is + it will be a delay) wrt the begin of the phase (OPTIONAL, default 0.0)
+#    -the field smoothingTime which defines the smoothing time with which the PIDs will be changed (OPTIONAL, default 1.0)
+
+# if 0 only the default group will be taken into consideration
+useGainScheduling           0
+
+[DEFAULT]
+#NAME                P     I      D
+#torso_pitch         5.0   1.0    0.5
+l_knee             -20000.0  -1000.0   0.0
+r_knee              20000.0   1000.0   0.0
+l_ankle_pitch      -15000.0  -1000.0   0.0
+r_ankle_pitch       15000.0   1000.0   0.0
+l_ankle_roll       -10000.0  -2000.0   0.0
+r_ankle_roll        10000.0   2000.0   0.0
+# l_hip_pitch        35.0  0.35   0.35
+# r_hip_pitch           35.0  0.35   0.35
+l_hip_roll           30000.0   1000.0   0.0
+r_hip_roll          -30000.0  -1000.0   0.0
+r_hip_yaw            5000.0    2000.0   0.0
+l_hip_yaw            -5000.0    -2000.0   0.0
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini
new file mode 100644
index 00000000..7db17e63
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini
@@ -0,0 +1,59 @@
+##Timings
+plannerHorizon          10.0
+
+##Unicycle Related Quantities
+unicycleGain            10.0
+referencePosition       (0.1 0.0)
+timeWeight              2.5
+positionWeight          1.0
+slowWhenTurningGain     5.0
+
+##Bounds
+#Step length
+maxStepLength           0.21
+minStepLength           0.05
+#Width
+minWidth                0.14
+#Angle Variations in DEGREES
+#maxAngleVariation       12.0
+maxAngleVariation       22.0
+minAngleVariation       8.0
+#Timings
+maxStepDuration         2.0
+minStepDuration         1.5
+
+##Nominal Values
+#Width
+nominalWidth            0.17
+#Height
+stepHeight              0.035
+stepLandingVelocity     0.0
+footApexTime            0.5
+comHeightDelta          0.01
+#Timings
+nominalDuration         1.9
+lastStepSwitchTime      0.8
+switchOverSwingRatio    0.7
+
+#ZMP Delta
+leftZMPDelta           (0.01 -0.0)
+rightZMPDelta          (0.01 -0.005)
+
+# Last Step DCM Offset
+# If it is 0.5 the final DCM will be in the middle of the two footsteps;
+# If it is 0 the DCM position coincides with the stance foot ZMP;
+# If it is 1 the DCM position coincides with the next foot ZMP position.
+lastStepDCMOffset      0.25
+
+#MergePoint
+mergePointRatio         0.4
+
+# pitch delta
+pitchDelta		0.0
+
+##Should be the first step with the left foot?
+swingLeft               1
+startAlwaysSameFoot     1
+
+##Remove this line if you don't want to use the minimum jerk trajectory in feet interpolation
+# useMinimumJerkFootTrajectory    1
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/walkingLogger.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/walkingLogger.ini
new file mode 100644
index 00000000..943ae735
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/walkingLogger.ini
@@ -0,0 +1,5 @@
+dataLoggerOutputPort_name         /logger/data:o
+dataLoggerRpcOutputPort_name      /logger/rpc:o
+
+dataLoggerInputPort_name          /logger/data:i
+dataLoggerRpcInputPort_name       /logger/rpc:i
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/zmpControllerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/zmpControllerParams.ini
new file mode 100644
index 00000000..2e2ed827
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/zmpControllerParams.ini
@@ -0,0 +1,20 @@
+# remove the following line if you do not want the gain scheduling
+useGainScheduling      1
+
+smoothingTime          0.05
+
+# if the gain scheduling is off only k*_walking parameters are used
+kZMP_walking           0.7
+# kZMP_stance            1.0
+kCoM_walking           4.0
+
+kZMP_stance            0.5
+# kZMP_stance            1.0
+kCoM_stance            4.0
+
+# old parameters
+#kZMP                    2.0 low velocity reactive
+#kZMP                    1.9 low velocity MPC
+
+#kCoM                    10.0 low velocity reactive
+#kCoM                    8.0 low velocity MPC
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/hand_retargeting/handRetargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/hand_retargeting/handRetargeting.ini
new file mode 100644
index 00000000..600d9797
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/hand_retargeting/handRetargeting.ini
@@ -0,0 +1,12 @@
+approaching_phase_duration      5.0
+
+[HAND_RETARGETING]
+
+left_hand_transform_port_name     /leftHandDesiredPose:i
+right_hand_transform_port_name    /rightHandDesiredPose:i
+
+smoothing_time_approaching       3.0
+smoothing_time_walking           0.1
+
+[VIRTUALIZER]
+robot_orientation_port_name     /torsoYaw:o
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/hand_retargeting/inverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/hand_retargeting/inverseKinematics.ini
new file mode 100644
index 00000000..496dabde
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/hand_retargeting/inverseKinematics.ini
@@ -0,0 +1,28 @@
+#The left_foot_frame is supposed to have the same orientation of the Inertia frame, which have the z axis pointing upwards,
+#the x axis pointing forward and and the y concludes a right-handed frame
+left_foot_frame         l_sole
+right_foot_frame        r_sole
+
+#Remove the following line in order not to consider the
+#additional frame
+additional_frame        neck_2
+
+#The additional rotation is defined (by rows) in the Inertia frame.
+#Remove the following line to keep the identity as additional rotation
+additional_rotation     ((1.0 0.0 0.0),(0.0 1.0 0.0),(0.0 0.0 1.0))
+
+# solver paramenters
+solver-verbosity        0
+# solver_name             ma27
+max-cpu-time            20
+
+#DEGREES
+jointRegularization     (7, 0.12, -0.01,
+                         -3.7, 25.0, -13.0, 40.769, 0, 0, 0,
+                         -3.7, 25.0, -13.0, 40.769, 0, 0, 0, 
+                         5.76, 1.61, -0.31, -31.64, -20.52, -1.52,
+                         5.76, 1.61, -0.31, -31.64, -20.52, -1.52)
+
+
+#jointRegularization     (0, 15, 0, 0, -29.794, 29.97, 0.00, 44.977, -29.794, 29.97, 0.00, 44.977, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351)
+#jointRegularization     (15, 0, 0, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351)
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/hand_retargeting/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/hand_retargeting/qpInverseKinematics.ini
new file mode 100644
index 00000000..bc2246d2
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/hand_retargeting/qpInverseKinematics.ini
@@ -0,0 +1,50 @@
+# CoM
+use_com_as_constraint             1
+# comWeightTriplets              ((0,0,100), (1,1,100), (2,2,100))
+
+# weight matrices related to the neck
+neck_weight                     5.0
+additional_rotation             ((1.0 0.0 0.0),(0.0 1.0 0.0),(0.0 0.0 1.0))
+
+## Regularization in degrees
+joint_regularization        (7, 0.12, -0.01,
+                             -3.7, 25.0, -13.0, 40.769, 0, 0, 0,
+                             -3.7, 25.0, -13.0, 40.769, 0, 0, 0, 
+                              5.76, 1.61, -0.31, -31.64, -20.52, -1.52,
+                              5.76, 1.61, -0.31, -31.64, -20.52, -1.52)
+
+joint_regularization_weights   (1.0, 1.0, 1.0,
+                                0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25,
+                                0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25,
+                                1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+
+joint_regularization_gains       (5.0, 5.0, 5.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0,
+                                 5.0, 5.0, 5.0, 5.0, 5.0, 5.0
+                                 5.0, 5.0, 5.0, 5.0, 5.0, 5.0)
+
+#Gains
+k_posCom                        1.0
+k_posFoot                       10.0
+k_attFoot                       5.0
+k_neck                          1.0
+
+# Hand retargeting
+k_posHand                       1.0
+k_attHand                       1.0
+# Upperbounds for the hand velocity defined by the controller and used as reference in the QP-IK. Negative value to disable the check
+max_hand_linear_vel_modulus     0.3
+max_hand_angular_vel_modulus    -1.0
+
+# use gain scheduling for hand retargeting
+smoothing_time                  0.5
+hand_weight_stance               (10, 10, 10, 2.0, 2.0, 2.0)
+hand_weight_walking              (10, 10, 10, 2.0, 2.0, 2.0)
+
+# joint limits
+k_joint_limit_lower_bound       1.0
+k_joint_limit_upper_bound       1.0
+
+# use_joint_limits_constraint   1
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/hand_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/hand_retargeting/robotControl.ini
new file mode 100644
index 00000000..b1381671
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/hand_retargeting/robotControl.ini
@@ -0,0 +1,32 @@
+robot                   icub
+
+
+joints_list             ("torso_pitch", "torso_roll", "torso_yaw",
+                        "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw",
+                        "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw",
+                        "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll",
+                        "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")
+
+remote_control_boards   ("torso", "left_arm", "right_arm", "left_leg", "right_leg")
+
+# filters
+# if use_*_filter is equal to 0 the low pass filters are not used
+use_joint_velocity_filter          0
+joint_velocity_cut_frequency       10.0
+
+use_wrench_filter                  0
+wrench_cut_frequency               10.0
+
+# if true the joint is in stiff mode if false the joint is in compliant mode
+joint_is_stiff_mode     (true, true, true,
+                         true, true, true, true, true, true, true,
+                         true, true, true, true, true, true, true,
+                         true, true, true, true, true, true,
+                         true, true, true, true, true, true)
+
+# if true a good joint traking is considered mandatory
+good_tracking_required  (true, true, true,
+                         true, true, true, true, false, false, false,
+                         true, true, true, true, false, false, false,
+                         true, true, true, true, true, true,
+                         true, true, true, true, true, true)
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joint_retargeting/inverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joint_retargeting/inverseKinematics.ini
new file mode 100644
index 00000000..c16d64e5
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joint_retargeting/inverseKinematics.ini
@@ -0,0 +1,28 @@
+#The left_foot_frame is supposed to have the same orientation of the Inertia frame, which have the z axis pointing upwards,
+#the x axis pointing forward and and the y concludes a right-handed frame
+left_foot_frame         l_sole
+right_foot_frame        r_sole
+
+#Remove the following line in order not to consider the
+#additional frame
+additional_frame        neck_2
+
+#The additional rotation is defined (by rows) in the Inertia frame.
+#Remove the following line to keep the identity as additional rotation
+additional_rotation     ((1.0 0.0 0.0),(0.0 1.0 0.0),(0.0 0.0 1.0))
+
+# solver paramenters
+solver-verbosity        0
+# solver_name             ma27
+max-cpu-time            20
+
+#DEGREES
+jointRegularization     (0.0, 0.0, 0.0,
+                         7, 0.12, -0.01,
+                         -3.7, 25.0, -13.0, 40.769, 0, 0, 0,
+                         -3.7, 25.0, -13.0, 40.769, 0, 0, 0,
+                         5.76, 1.61, -0.31, -31.64, -20.52, -1.52,
+                         5.76, 1.61, -0.31, -31.64, -20.52, -1.52)
+
+#jointRegularization     (0, 15, 0, 0, -29.794, 29.97, 0.00, 44.977, -29.794, 29.97, 0.00, 44.977, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351)
+#jointRegularization     (15, 0, 0, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351)
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joint_retargeting/jointRetargeting.ini
new file mode 100644
index 00000000..d6fbcf5c
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joint_retargeting/jointRetargeting.ini
@@ -0,0 +1,28 @@
+approaching_phase_duration      4.0
+
+[HAND_RETARGETING]
+
+
+[JOINT_RETARGETING]
+## List of the retargeting joint. This list must be the same or a subset of the
+## "joints_list" in robotControl.ini. The order of the joints should be choseen
+## accordingly to the order of the joints received in the
+## "joint_retargeting_port_name" port
+retargeting_joint_list          ("neck_pitch", "neck_roll", "neck_yaw",
+                                 "torso_pitch", "torso_roll", "torso_yaw",
+                                 "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup",  "l_wrist_pitch", "l_wrist_yaw",
+                                 "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw")
+
+
+joint_retargeting_port_name     /jointPosition:i
+smoothing_time_approaching       2.0
+smoothing_time_walking           0.2
+
+[VIRTUALIZER]
+robot_orientation_port_name     /torsoYaw:o
+
+[COM_RETARGETING]
+com_height_retargeting_port_name /CoM:i
+smoothing_time_approaching       2.0
+smoothing_time_walking           1.0
+com_height_scaling_factor        0.5
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joint_retargeting/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joint_retargeting/qpInverseKinematics.ini
new file mode 100644
index 00000000..0b20d9bc
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joint_retargeting/qpInverseKinematics.ini
@@ -0,0 +1,78 @@
+# CoM
+use_com_as_constraint             1
+
+# torso
+torso_weight_walking            5.0
+torso_weight_stance             0.0
+additional_rotation             ((1.0 0.0 0.0),(0.0 1.0 0.0),(0.0 0.0 1.0))
+
+# joint regularization values in degrees
+joint_regularization            (0.0, 0.0, 0.0,
+                                 7, 0.12, -0.01,
+                                -3.7, 25.0, -13.0, 40.769, 0, 0, 0,
+                                -3.7, 25.0, -13.0, 40.769, 0, 0, 0,
+                                 5.76, 1.61, -0.31, -31.64, -20.52, -1.52,
+                                 5.76, 1.61, -0.31, -31.64, -20.52, -1.52)
+
+joint_regularization_gains       (5.0, 5.0, 5.0,
+                                  5.0, 5.0, 5.0,
+                                  1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0,
+                                  1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0,
+                                  5.0, 5.0, 5.0, 5.0, 5.0, 5.0
+                                  5.0, 5.0, 5.0, 5.0, 5.0, 5.0)
+
+joint_regularization_weight_stance  (0.0, 0.0, 0.0,
+                                     0.0, 0.0, 0.0,
+                                     0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+                                     0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+                                     0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+                                     0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+
+joint_regularization_weight_walking (0.0, 0.0, 0.0,
+                                     1.0, 1.0, 1.0,
+                                     0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+                                     0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+                                     1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                     1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+
+## Retargeting
+joint_retargeting_gains         (5.0, 5.0, 5.0,
+                                 5.0, 5.0, 5.0,
+                                 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0,
+                                 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0,
+                                 5.0, 5.0, 5.0, 5.0, 5.0, 5.0
+                                 5.0, 5.0, 5.0, 5.0, 5.0, 5.0)
+
+joint_retargeting_weight_walking (1.0, 1.0, 1.0,
+                                  0.0, 0.0, 0.0,
+                                  2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0,
+                                  2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0,
+                                  1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                  1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+
+joint_retargeting_weight_stance  (1.0, 1.0, 1.0,
+                                  1.0, 1.0, 1.0,
+                                  2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0,
+                                  2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0,
+                                  1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                  1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+
+
+#Gains
+k_posCom                        1.0
+k_posFoot                       7.0
+k_attFoot                       5.0
+k_neck                          5.0
+
+# Hand retargeting
+k_posHand                       2.5
+k_attHand                       1.0
+
+# use gain scheduling for hand retargeting
+smoothing_time                  0.5
+
+# joint limits
+k_joint_limit_lower_bound       1.0
+k_joint_limit_upper_bound       1.0
+
+# use_joint_limits_constraint   1
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joint_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joint_retargeting/robotControl.ini
new file mode 100644
index 00000000..3bfcc54d
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joint_retargeting/robotControl.ini
@@ -0,0 +1,36 @@
+robot                   icub
+
+
+joints_list             ("neck_pitch", "neck_roll", "neck_yaw",
+                         "torso_pitch", "torso_roll", "torso_yaw",
+                         "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw",
+                         "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw",
+                         "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll",
+                         "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")
+
+remote_control_boards   ("head", "torso", "left_arm", "right_arm", "left_leg", "right_leg")
+
+# filters
+# if use_*_filter is equal to 0 the low pass filters are not used
+use_joint_velocity_filter          0
+joint_velocity_cut_frequency       10.0
+
+use_wrench_filter                  0
+wrench_cut_frequency               10.0
+
+
+# if true the joint is in stiff mode if false the joint is in compliant mode
+joint_is_stiff_mode     (true, true, true,
+                         true, true, true,
+                         true, true, true, true, true, true, true,
+                         true, true, true, true, true, true, true,
+                         true, true, true, true, true, true,
+                         true, true, true, true, true, true)
+
+# if true a good joint traking is considered mandatory
+good_tracking_required  (false, false, false,
+                         true, true, true,
+                         true, true, true, true, false, false, false,
+                         true, true, true, true, false, false, false,
+                         true, true, true, true, true, true,
+                         true, true, true, true, true, true)
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/handRetargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/handRetargeting.ini
new file mode 100644
index 00000000..8c9da41c
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/handRetargeting.ini
@@ -0,0 +1,2 @@
+# This file is empty because when the robot is controller using only the joypad
+# the retargeting is disabled
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/inverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/inverseKinematics.ini
new file mode 100644
index 00000000..0c1232bd
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/inverseKinematics.ini
@@ -0,0 +1,35 @@
+#The left_foot_frame is supposed to have the same orientation of the Inertia frame, which have the z axis pointing upwards,
+#the x axis pointing forward and and the y concludes a right-handed frame
+left_foot_frame         l_sole
+right_foot_frame        r_sole
+
+# #Remove the following line in order not to consider the
+# #additional frame
+additional_frame        chest
+
+# #The additional rotation is defined (by rows) in the Inertia frame.
+#Remove the following line to keep the identity as additional rotation
+additional_rotation     ((1.0 0.0 0.0),(0.0 1.0 0.0),(0.0 0.0 1.0))
+
+# solver paramenters
+solver-verbosity        1
+# solver_name             ma27
+max-cpu-time            20
+
+
+# joints_list             "torso_pitch", "torso_roll", "torso_yaw",
+#                         "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow",
+#                         "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow",
+#                         "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll",
+#                         "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll"
+
+
+#DEGREES
+jointRegularization     (7, 0.12, -0.01,
+                         -3.7, 20.0, -13.0, 40.769,
+                         -3.7, 20.0, -13.0, 40.769,
+                         5.76, 1.61, -0.31, -31.64, -20.52, -1.52,
+                         5.76, 1.61, -0.31, -31.64, -20.52, -1.52)
+
+#jointRegularization     (0, 15, 0, 0, -29.794, 29.97, 0.00, 44.977, -29.794, 29.97, 0.00, 44.977, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351)
+#jointRegularization     (15, 0, 0, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351)
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/qpInverseKinematics.ini
new file mode 100644
index 00000000..3b6d9da7
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/qpInverseKinematics.ini
@@ -0,0 +1,38 @@
+# CoM
+use_com_as_constraint           1
+# comWeightTriplets              ((0,0,100), (1,1,100), (2,2,100))
+
+# weight matrices related to the neck
+neck_weight                     5.0
+additional_rotation             ((1.0 0.0 0.0),(0.0 1.0 0.0),(0.0 0.0 1.0))
+
+# Joint regularization term
+joint_regularization     (7, 0.12, -0.01,
+                         -3.7, 20.0, -13.0, 40.769,
+                         -3.7, 20.0, -13.0, 40.769,
+                         5.76, 1.61, -0.31, -31.64, -20.52, -1.52,
+                         5.76, 1.61, -0.31, -31.64, -20.52, -1.52)
+
+joint_regularization_weights     (1.0, 1.0, 1.0,
+                               2.0, 2.0, 2.0, 2.0,
+                               2.0, 2.0, 2.0, 2.0,
+                               1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                               1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+
+joint_regularization_gains     (5.0, 5.0, 5.0,
+                               5.0, 5.0, 5.0, 5.0,
+                               5.0, 5.0, 5.0, 5.0,
+                               5.0, 5.0, 5.0, 5.0, 5.0, 5.0,
+                               5.0, 5.0, 5.0, 5.0, 5.0, 5.0)
+
+# Gains
+k_posCom                        1.0
+k_posFoot                       7.0
+k_attFoot                       5.0
+k_neck                          5.0
+
+# joint limits
+k_joint_limit_lower_bound       1.0
+k_joint_limit_upper_bound       1.0
+
+# use_joint_limits_constraint   1
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/robotControl.ini
new file mode 100644
index 00000000..ee1cc64d
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/robotControl.ini
@@ -0,0 +1,32 @@
+robot                   icub
+
+joints_list             ("torso_pitch", "torso_roll", "torso_yaw",
+                        "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow",
+                        "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow",
+                        "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll",
+                        "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")
+
+remote_control_boards   ("torso", "left_arm", "right_arm", "left_leg", "right_leg")
+
+# filters
+# if use_*_filter is equal to 0 the low pass filters are not used
+use_joint_velocity_filter          0
+joint_velocity_cut_frequency       10.0
+
+use_wrench_filter                  0
+wrench_cut_frequency               10.0
+
+
+# if true the joint is in stiff mode if false the joint is in compliant mode
+joint_is_stiff_mode     (true, true, true,
+                         true, true, true, true,
+                         true, true, true, true,
+                         true, true, true, true, true, true,
+                         true, true, true, true, true, true)
+
+# if true a good joint traking is considered mandatory
+good_tracking_required  (true, true, true,
+                         true, true, true, false,
+                         true, true, true, false,
+                         true, true, true, true, true, true,
+                         true, true, true, true, true, true)
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_hand_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_hand_retargeting.ini
new file mode 100644
index 00000000..458fdd1e
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_hand_retargeting.ini
@@ -0,0 +1,60 @@
+# Remove this line if you don't want to use the MPC
+# use_mpc                            1
+
+# Remove this line if you don't want to use the QP-IK
+use_QP-IK                          1
+
+# Remove this line if you don't want to use osqp to
+# solve QP-IK. In this case qpOASES will be used
+use_osqp                           1
+
+# remove this line if you don't want to save data of the experiment
+# dump_data                          1
+
+# general parameters
+[GENERAL]
+name                    walking-coordinator
+# height of the com
+com_height              0.53
+# sampling time
+sampling_time           0.01
+# enable hand retargeting
+use_hand_retargeting    1
+# enable the virtualizer
+use_virtualizer         1
+
+# include robot control parameters
+[include ROBOT_CONTROL "./dcm_walking/hand_retargeting/robotControl.ini"]
+
+# include trajectory planner parameters
+[include TRAJECTORY_PLANNER "./dcm_walking/common/plannerParams.ini"]
+
+# include MPC parameters
+[include DCM_MPC_CONTROLLER "./dcm_walking/common/controllerParams.ini"]
+
+# include MPC parameters
+[include DCM_REACTIVE_CONTROLLER "./dcm_walking/common/dcmReactiveControllerParams.ini"]
+
+# include MPC parameters
+[include ZMP_CONTROLLER "./dcm_walking/common/zmpControllerParams.ini"]
+
+# include inverse kinematcs parameters
+[include INVERSE_KINEMATICS_SOLVER "./dcm_walking/hand_retargeting/inverseKinematics.ini"]
+
+# include qp inverse kinematcs parameters
+[include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/hand_retargeting/qpInverseKinematics.ini"]
+
+# include inverse kinematcs parameters
+[include FORWARD_KINEMATICS_SOLVER "./dcm_walking/common/forwardKinematics.ini"]
+
+# include FT sensors parameters
+[include FT_SENSORS "./dcm_walking/common/forceTorqueSensors.ini"]
+
+# include Logger parameters
+[include WALKING_LOGGER "./dcm_walking/common/walkingLogger.ini"]
+
+# include lower PID parameters
+[include PID "./dcm_walking/common/pidParams.ini"]
+
+# retargeting
+[include RETARGETING "./dcm_walking/hand_retargeting/handRetargeting.ini"]
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_joint_retargeting.ini
new file mode 100644
index 00000000..982ad9cc
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_joint_retargeting.ini
@@ -0,0 +1,61 @@
+# Remove this line if you don't want to use the MPC
+# use_mpc                            1
+
+# Remove this line if you don't want to use the QP-IK
+use_QP-IK                          1
+
+# Remove this line if you don't want to use osqp to
+# solve QP-IK. In this case qpOASES will be used
+use_osqp                           1
+
+# remove this line if you don't want to save data of the experiment
+#dump_data                          1
+
+# general parameters
+[GENERAL]
+name                    walking-coordinator
+# height of the com
+com_height              0.53
+# sampling time
+sampling_time           0.01
+# enable joint retargeting
+use_joint_retargeting    1
+# enable the virtualizer
+use_virtualizer         1
+use_com_retargeting     1
+
+# include robot control parameters
+[include ROBOT_CONTROL "./dcm_walking/joint_retargeting/robotControl.ini"]
+
+# include trajectory planner parameters
+[include TRAJECTORY_PLANNER "./dcm_walking/common/plannerParams.ini"]
+
+# include MPC parameters
+[include DCM_MPC_CONTROLLER "./dcm_walking/common/controllerParams.ini"]
+
+# include MPC parameters
+[include DCM_REACTIVE_CONTROLLER "./dcm_walking/common/dcmReactiveControllerParams.ini"]
+
+# include MPC parameters
+[include ZMP_CONTROLLER "./dcm_walking/common/zmpControllerParams.ini"]
+
+# include inverse kinematcs parameters
+[include INVERSE_KINEMATICS_SOLVER "./dcm_walking/joint_retargeting/inverseKinematics.ini"]
+
+# include qp inverse kinematcs parameters
+[include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/joint_retargeting/qpInverseKinematics.ini"]
+
+# include inverse kinematcs parameters
+[include FORWARD_KINEMATICS_SOLVER "./dcm_walking/common/forwardKinematics.ini"]
+
+# include FT sensors parameters
+[include FT_SENSORS "./dcm_walking/common/forceTorqueSensors.ini"]
+
+# include Logger parameters
+[include WALKING_LOGGER "./dcm_walking/common/walkingLogger.ini"]
+
+# include lower PID parameters
+[include PID "./dcm_walking/common/pidParams.ini"]
+
+# retargeting
+[include RETARGETING "./dcm_walking/joint_retargeting/jointRetargeting.ini"]
diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini
new file mode 100644
index 00000000..1b43a0c6
--- /dev/null
+++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini
@@ -0,0 +1,53 @@
+# Remove this line if you don't want to use the MPC
+# use_mpc                            1
+
+# Remove this line if you don't want to use the QP-IK
+use_QP-IK                          1
+
+# Remove this line if you don't want to use osqp to
+# solve QP-IK. In this case qpOASES will be used
+use_osqp                           1
+
+# remove this line if you don't want to save data of the experiment
+# dump_data                          1
+
+# general parameters
+[GENERAL]
+name                    walking-coordinator
+# height of the com
+com_height              0.535
+# sampling time
+sampling_time           0.01
+
+# include robot control parameters
+[include ROBOT_CONTROL "./dcm_walking/joypad_control/robotControl.ini"]
+
+# include trajectory planner parameters
+[include TRAJECTORY_PLANNER "./dcm_walking/common/plannerParams.ini"]
+
+# include MPC parameters
+[include DCM_MPC_CONTROLLER "./dcm_walking/common/controllerParams.ini"]
+
+# include MPC parameters
+[include DCM_REACTIVE_CONTROLLER "./dcm_walking/common/dcmReactiveControllerParams.ini"]
+
+# include MPC parameters
+[include ZMP_CONTROLLER "./dcm_walking/common/zmpControllerParams.ini"]
+
+# include inverse kinematcs parameters
+[include INVERSE_KINEMATICS_SOLVER "./dcm_walking/joypad_control/inverseKinematics.ini"]
+
+# include qp inverse kinematcs parameters
+[include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/joypad_control/qpInverseKinematics.ini"]
+
+# include inverse kinematcs parameters
+[include FORWARD_KINEMATICS_SOLVER "./dcm_walking/common/forwardKinematics.ini"]
+
+# include FT sensors parameters
+[include FT_SENSORS "./dcm_walking/common/forceTorqueSensors.ini"]
+
+# include Logger parameters
+[include WALKING_LOGGER "./dcm_walking/common/walkingLogger.ini"]
+
+# include lower PID parameters
+[include PID "./dcm_walking/common/pidParams.ini"]
diff --git a/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking/common/forceTorqueSensors.ini b/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking/common/forceTorqueSensors.ini
index 3326b621..5590bb48 100644
--- a/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking/common/forceTorqueSensors.ini
+++ b/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking/common/forceTorqueSensors.ini
@@ -1,6 +1,6 @@
 # name of the left and right feet wrench ports
-leftFootWrenchInputPort_name     /leftFootWrench:i
-rightFootWrenchInputPort_name    /rightFootWrench:i
+left_foot_wrench_input_port_name     ("/left_foot/cartesianEndEffectorWrench:i")
+left_foot_wrench_output_port_name    ("/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o")
 
-leftFootWrenchOutputPort_name     /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o
-rightFootWrenchOutputPort_name    /wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o
+right_foot_wrench_input_port_name    ("/right_foot/cartesianEndEffectorWrench:i")
+right_foot_wrench_output_port_name   ("/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o")
diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp
index 92a0d82b..eb2a7a18 100644
--- a/src/WalkingModule/src/Module.cpp
+++ b/src/WalkingModule/src/Module.cpp
@@ -817,7 +817,7 @@ bool WalkingModule::updateModule()
                                       m_leftTrajectory.front().getPosition(), m_leftTrajectory.front().getRotation().asRPY(),
                                       m_rightTrajectory.front().getPosition(), m_rightTrajectory.front().getRotation().asRPY(),
                                       m_robotControlHelper->getJointPosition(),
-                                      m_retargetingClient->jointValues());
+                                      m_qDesired);
         }
 
         // in the approaching phase the robot should not move and the trajectories should not advance
@@ -992,6 +992,8 @@ bool WalkingModule::prepareRobot(bool onTheFly)
         return false;
     }
 
+    std::cerr << "q desired IK " << Eigen::VectorXd(iDynTree::toEigen(m_qDesired) * 180 / M_PI).transpose() << std::endl;
+
     if(!m_robotControlHelper->setPositionReferences(m_qDesired, 5.0))
     {
         yError() << "[WalkingModule::prepareRobot] Error while setting the initial position.";
@@ -1220,16 +1222,14 @@ bool WalkingModule::startWalking()
                     "lf_des_roll", "lf_des_pitch", "lf_des_yaw",
                     "rf_des_x", "rf_des_y", "rf_des_z",
                     "rf_des_roll", "rf_des_pitch", "rf_des_yaw",
-                    "neck_pitch", "neck_roll", "neck_yaw",
                     "torso_pitch", "torso_roll", "torso_yaw",
-                    "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup",
-                    "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup",
+                    "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow",
+                    "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow",
                     "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll",
                     "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll",
-                    "neck_pitch_des", "neck_roll_des", "neck_yaw_des",
                     "torso_pitch_des", "torso_roll_des", "torso_yaw_des",
-                    "l_shoulder_pitch_des", "l_shoulder_roll_des", "l_shoulder_yaw_des", "l_elbow_des", "l_wrist_prosup_des",
-                    "r_shoulder_pitch_des", "r_shoulder_roll_des", "r_shoulder_yaw_des", "r_elbow_des", "r_wrist_prosup_des",
+                    "l_shoulder_pitch_des", "l_shoulder_roll_des", "l_shoulder_yaw_des", "l_elbow_des",
+                    "r_shoulder_pitch_des", "r_shoulder_roll_des", "r_shoulder_yaw_des", "r_elbow_des",
                     "l_hip_pitch_des", "l_hip_roll_des", "l_hip_yaw_des", "l_knee_des", "l_ankle_pitch_des", "l_ankle_roll_des",
                     "r_hip_pitch_des", "r_hip_roll_des", "r_hip_yaw_des", "r_knee_des", "r_ankle_pitch_des", "r_ankle_roll_des"});
     }