Revamp of COM retargeting

src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h

@@ -98,6 +98,11 @@ class RetargetingClient
     /** Factor required to scale the human CoM displacement to a desired robot CoM displacement */
     double m_comHeightScalingFactor;
 
+    /** Flag to check if we received the initial value for the CoM height. */
+    bool m_comOffsetSet{ false };
+
+    iDynTree::Vector2 m_comVariationRange; /**< Range of variation of the CoM height */
+
     /** Mapping between the retarget joints and the controlled. */
     std::unordered_map<std::string, int> m_retargetedJointsToControlJoints;
 

src/RetargetingHelper/src/Helper.cpp

@@ -10,6 +10,8 @@
 #include <WalkingControllers/YarpUtilities/Helper.h>
 #include <WalkingControllers/RetargetingHelper/Helper.h>
 
+#include <algorithm>
+
 using namespace WalkingControllers;
 
 void RetargetingClient::convertYarpVectorPoseIntoTransform(const yarp::sig::Vector& vector,
@@ -51,7 +53,7 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config,
     m_hdeRetargeting.joints.position.resize(controlledJointNames.size());
     m_hdeRetargeting.joints.velocity.resize(controlledJointNames.size());
     m_hdeRetargeting.joints.smoother.yarpBuffer.resize(controlledJointNames.size());
-    m_hdeRetargeting.com.smoother.yarpBuffer(1);
+    m_hdeRetargeting.com.smoother.yarpBuffer.resize(1);
 
     if(!m_useHandRetargeting && !m_useVirtualizer &&
        !m_useJointRetargeting && !m_useCoMHeightRetargeting)
@@ -206,6 +208,25 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config,
             yError() << "[RetargetingClient::initialize] Unable to get the number from searchable.";
             return false;
         }
+
+        if (!YarpUtilities::getVectorFromSearchable(option, "variation_range", m_comVariationRange))
+        {
+            yError() << "[RetargetingClient::initialize] Initialization failed while reading range vector.";
+            return false;
+        }
+
+        if (m_comVariationRange(0) > 0 || m_comVariationRange(1) < 0)
+        {
+            yError() << "[RetargetingClient::initialize] The range is not valid. The range has to contain the zero.";
+            return false;
+        }
+
+        if (m_comVariationRange(0) > m_comVariationRange(1))
+        {
+            yError() << "[RetargetingClient::initialize] The range is not valid. The first element has to be smaller than the second one.";
+            return false;
+        }
+
     }
 
     if (m_useJointRetargeting || m_useCoMHeightRetargeting)
@@ -257,37 +278,12 @@ bool RetargetingClient::reset(WalkingFK& kinDynWrapper)
     m_hdeRetargeting.com.position = kinDynWrapper.getCoMPosition()(2);
     m_hdeRetargeting.com.velocity = 0;
     m_comConstantHeight = m_hdeRetargeting.com.position;
+    m_comOffsetSet = false;
 
     if(m_useCoMHeightRetargeting)
     {
-        m_hdeRetargeting.com.smoother.yarpBuffer(0) = m_hdeRetargeting.com.position;
-        m_hdeRetargeting.joints.smoother.smoother->init(m_hdeRetargeting.com.smoother.yarpBuffer);
-
-        // let's read the port to reset the comHeightInput
-        bool okCoMHeight = false;
-        unsigned int attempt = 0;
-        do
-        {
-            if(!okCoMHeight)
-            {
-                auto data = m_hdeRetargeting.port.read(false);
-                if(data != nullptr)
-                {
-                    m_comHeightInputOffset = data->CoMPositionWRTGlobal.z;
-                    okCoMHeight = true;
-                }
-            }
-
-            if(okCoMHeight)
-                return true;
-
-            yarp::os::Time::delay(0.001);
-            attempt++;
-        } while (attempt < 100);
-
-        if(!okCoMHeight)
-            yError() << "[RetargetingClient::reset] The CoM height is not coming from the yarp port.";
-        return false;
+        m_hdeRetargeting.com.smoother.yarpBuffer(0) = m_comConstantHeight;
+        m_hdeRetargeting.com.smoother.smoother->init(m_hdeRetargeting.com.smoother.yarpBuffer);
     }
 
     return true;
@@ -331,15 +327,20 @@ bool RetargetingClient::getFeedback()
             this->enableApproachingIfNecessary();
             if (m_useCoMHeightRetargeting)
             {
+                if (!m_comOffsetSet)
+                {
+                    m_comHeightInputOffset = HDEData->baseOriginWRTGlobal.z;
+                    m_comOffsetSet = true;
+                }
                 if (m_phase == Phase::Walking)
                 {
                     m_hdeRetargeting.com.smoother.yarpBuffer(0) = m_comConstantHeight;
                 } else
                 {
-                    const auto& desiredCoMHeight = HDEData->CoMPositionWRTGlobal.z;
-                    m_hdeRetargeting.com.smoother.yarpBuffer(0)
-                        = (desiredCoMHeight - m_comHeightInputOffset) * m_comHeightScalingFactor
-                          + m_comConstantHeight;
+                    const auto& desiredCoMHeight = HDEData->baseOriginWRTGlobal.z;
+                    double variation = (desiredCoMHeight - m_comHeightInputOffset) * m_comHeightScalingFactor;
+                    double clampedVariation = std::clamp(variation, m_comVariationRange(0), m_comVariationRange(1));
+                    m_hdeRetargeting.com.smoother.yarpBuffer(0) = clampedVariation + m_comConstantHeight;
                 }
             }
 
@@ -379,7 +380,9 @@ bool RetargetingClient::getFeedback()
                 {
                     this->setPhase(Phase::Approaching);
                     m_retargetedJointsToHDEJoints.clear();
+                    m_comOffsetSet = false;
                     iDynTree::toEigen(m_hdeRetargeting.joints.smoother.yarpBuffer) = iDynTree::toEigen(m_hdeRetargeting.joints.initialState);
+                    m_hdeRetargeting.com.smoother.yarpBuffer(0) = m_comConstantHeight;
                 }
             }
         }

src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini

@@ -25,3 +25,4 @@ robot_orientation_port_name     /torsoYaw:o
 smoothing_time_approaching       2.0
 smoothing_time_walking           1.0
 com_height_scaling_factor        0.5
+variation_range                  (-0.1 0.0)

src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini

@@ -25,3 +25,4 @@ robot_orientation_port_name     /torsoYaw:o
 smoothing_time_approaching       2.0
 smoothing_time_walking           1.0
 com_height_scaling_factor        0.5
+variation_range                  (-0.1 0.0)

src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/jointRetargeting.ini

@@ -25,3 +25,4 @@ robot_orientation_port_name     /torsoYaw:o
 smoothing_time_approaching       2.0
 smoothing_time_walking           1.0
 com_height_scaling_factor        0.5
+variation_range                  (-0.05 0.0)

src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini

@@ -25,3 +25,4 @@ robot_orientation_port_name     /torsoYaw:o
 smoothing_time_approaching       2.0
 smoothing_time_walking           1.0
 com_height_scaling_factor        0.5
+variation_range                  (-0.1 0.0)

src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/jointRetargeting.ini

@@ -25,3 +25,4 @@ robot_orientation_port_name     /torsoYaw:o
 smoothing_time_approaching       2.0
 smoothing_time_walking           1.0
 com_height_scaling_factor        0.5
+variation_range                  (-0.05 0.0)

src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini

@@ -25,3 +25,4 @@ robot_orientation_port_name     /torsoYaw:o
 smoothing_time_approaching       2.0
 smoothing_time_walking           1.0
 com_height_scaling_factor        0.5
+variation_range                  (-0.1 0.0)

src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini

@@ -28,3 +28,4 @@ robot_orientation_port_name     /torsoYaw:o
 com_height_retargeting_port_name /CoM:i
 smoothing_time_approaching       2.0
 smoothing_time_walking           1.0
+variation_range                  (-0.05 0.0)

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/jointRetargeting.ini

@@ -28,3 +28,4 @@ com_height_retargeting_port_name /CoM:i
 smoothing_time_approaching       2.0
 smoothing_time_walking           1.0
 com_height_scaling_factor        0.5
+variation_range                  (-0.05 0.0)

src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini

@@ -27,3 +27,4 @@ com_height_retargeting_port_name /CoM:i
 smoothing_time_approaching       2.0
 smoothing_time_walking           1.0
 com_height_scaling_factor        0.5
+variation_range                  (-0.05 0.0)

src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini

@@ -25,3 +25,4 @@ robot_orientation_port_name     /torsoYaw:o
 smoothing_time_approaching       2.0
 smoothing_time_walking           1.0
 com_height_scaling_factor        0.5
+variation_range                  (-0.05 0.0)