Add the possibility to retarget the CoM forward direction

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

@@ -10,6 +10,7 @@
 #define WALKING_CONTROLLERS_RETARGETING_HELPER_HELPER_H
 
 // std
+#include <iDynTree/Core/VectorFixSize.h>
 #include <memory>
 #include <vector>
 
@@ -73,7 +74,8 @@ class RetargetingClient
     struct HDERetargeting : public RetargetingElement<WalkingControllers::YarpUtilities::HumanState>
     {
         KinematicState<iDynTree::VectorDynSize> joints;
-        KinematicState<double> com;
+        KinematicState<double> comHeight;
+        KinematicState<iDynTree::Vector2> comHorizontal;
     };
 
     struct HandRetargeting : public RetargetingElement<yarp::sig::Vector>
@@ -85,7 +87,8 @@ class RetargetingClient
     bool m_useHandRetargeting; /**< True if the hand retargeting is used */
     bool m_useVirtualizer; /**< True if the virtualizer is used */
     bool m_useJointRetargeting; /**< True if the joint retargeting is used */
-    bool m_useCoMHeightRetargeting; /**< True if the com retargeting is used */
+    bool m_useCoMHeightRetargeting; /**< True if the com height retargeting is used */
+    bool m_useCoMHorizontalRetargeting; /**< True if the horizontal com retargeting is used */
 
     HandRetargeting m_leftHand; /**< Left hand retargeting element */
     HandRetargeting m_rightHand; /**< Right hand retargeting element */
@@ -101,6 +104,11 @@ class RetargetingClient
     /** Factor required to scale the human CoM displacement to a desired robot CoM displacement */
     double m_comHeightScalingFactor;
 
+    iDynTree::Vector2 m_comHorizontalInputOffset;
+    iDynTree::Vector2 m_comXLimits;
+    iDynTree::Vector2 m_comYLimits;
+    bool m_comHorizontalRetargetingReady{false};
+
     /** Mapping between the retarget joints and the controlled. */
     std::unordered_map<std::string, int> m_retargetedJointsToControlJoints;
 
@@ -181,6 +189,13 @@ class RetargetingClient
      */
     const iDynTree::VectorDynSize& jointVelocities() const;
 
+
+    /**
+     * Get the position of com horizontal position
+     */
+    const iDynTree::Vector2& comHorizonal() const;
+
+
     /**
      * Get the CoM height
      * @return height of the CoM

src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h

@@ -57,6 +57,9 @@ namespace WalkingControllers
 
         iDynTree::Vector2 m_referencePointDistance; /**< Vector between the center of the unicycle and the point that has to be reach the goal. */
 
+        iDynTree::Vector2 m_leftZMPIntialDelta;
+        iDynTree::Vector2 m_rightZMPIntialDelta;
+
         GeneratorState m_generatorState{GeneratorState::NotConfigured}; /**< Useful to track the generator state. */
 
         std::thread m_generatorThread; /**< Main trajectory thread. */
@@ -135,7 +138,8 @@ namespace WalkingControllers
          */
         bool updateTrajectories(double initTime, const iDynTree::Vector2& DCMBoundaryConditionAtMergePointPosition,
                                 const iDynTree::Vector2& DCMBoundaryConditionAtMergePointVelocity, bool correctLeft,
-                                const iDynTree::Transform& measured, const iDynTree::VectorDynSize& plannerDesiredInput);
+                                const iDynTree::Transform& measured, const iDynTree::VectorDynSize& plannerDesiredInput,
+                                const iDynTree::Vector2& leftZMPOffsetDelta, const iDynTree::Vector2& rightZMPOffsetDelta);
 
         /**
          * @brief Set the free space ellipse in which the robot can walk.

src/TrajectoryPlanner/src/TrajectoryGenerator.cpp

@@ -12,7 +12,9 @@
 
 // iDynTree
 #include <iDynTree/Core/EigenHelpers.h>
+#include <iDynTree/Core/VectorFixSize.h>
 
+// walking-controllers
 #include <WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h>
 #include <WalkingControllers/YarpUtilities/Helper.h>
 
@@ -66,15 +68,13 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config)
     }
 
     // get left and right ZMP delta
-    iDynTree::Vector2 leftZMPDelta;
-    if(!YarpUtilities::getVectorFromSearchable(config, "leftZMPDelta", leftZMPDelta))
+    if(!YarpUtilities::getVectorFromSearchable(config, "leftZMPDelta", m_leftZMPIntialDelta))
     {
         yError() << "[configurePlanner] Initialization failed while reading leftZMPDelta vector.";
         return false;
     }
 
-    iDynTree::Vector2 rightZMPDelta;
-    if(!YarpUtilities::getVectorFromSearchable(config, "rightZMPDelta", rightZMPDelta))
+    if(!YarpUtilities::getVectorFromSearchable(config, "rightZMPDelta", m_rightZMPIntialDelta))
     {
         yError() << "[configurePlanner] Initialization failed while reading rightZMPDelta vector.";
         return false;
@@ -203,7 +203,7 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config)
     ok = ok && m_trajectoryGenerator.setMergePointRatio(mergePointRatios[0], mergePointRatios[1]);
 
     m_dcmGenerator = m_trajectoryGenerator.addDCMTrajectoryGenerator();
-    m_dcmGenerator->setFootOriginOffset(leftZMPDelta, rightZMPDelta);
+    m_dcmGenerator->setFootOriginOffset(m_leftZMPIntialDelta, m_rightZMPIntialDelta);
     m_dcmGenerator->setOmega(sqrt(9.81/comHeight));
     m_dcmGenerator->setFirstDCMTrajectoryMode(FirstDCMTrajectoryMode::FifthOrderPoly);
     ok = ok && m_dcmGenerator->setLastStepDCMOffsetPercentage(lastStepDCMOffset);
@@ -565,7 +565,8 @@ bool TrajectoryGenerator::generateFirstTrajectories(const iDynTree::Transform &l
 
 bool TrajectoryGenerator::updateTrajectories(double initTime, const iDynTree::Vector2& DCMBoundaryConditionAtMergePointPosition,
                                              const iDynTree::Vector2& DCMBoundaryConditionAtMergePointVelocity, bool correctLeft,
-                                             const iDynTree::Transform& measured, const iDynTree::VectorDynSize &plannerDesiredInput)
+                                             const iDynTree::Transform& measured, const iDynTree::VectorDynSize &plannerDesiredInput,
+                                             const iDynTree::Vector2& leftZMPOffsetDelta, const iDynTree::Vector2& rightZMPOffsetDelta)
 {
     {
         std::lock_guard<std::mutex> guard(m_mutex);
@@ -587,6 +588,12 @@ bool TrajectoryGenerator::updateTrajectories(double initTime, const iDynTree::Ve
         m_personFollowingDesiredPoint.zero();
         m_desiredDirectControl.zero();
 
+        iDynTree::Vector2 leftZMPDelta = m_leftZMPIntialDelta;
+        iDynTree::Vector2 rightZMPDelta = m_rightZMPIntialDelta;
+        iDynTree::toEigen(leftZMPDelta) += iDynTree::toEigen(leftZMPOffsetDelta);
+        iDynTree::toEigen(rightZMPDelta) += iDynTree::toEigen(rightZMPOffsetDelta);
+        m_dcmGenerator->setFootOriginOffset(leftZMPDelta, rightZMPDelta);
+
         if (m_unicycleController == UnicycleController::PERSON_FOLLOWING)
         {
             if (plannerDesiredInput.size() < 2)

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

@@ -20,7 +20,14 @@ smoothing_time_walking           0.03
 [VIRTUALIZER]
 robot_orientation_port_name     /torsoYaw:o
 
-[COM_RETARGETING]
+[COM_HEIGHT_RETARGETING]
 smoothing_time_approaching       2.0
 smoothing_time_walking           1.0
 com_height_scaling_factor        0.5
+
+
+[COM_HORIZONTAL_RETARGETING]
+smoothing_time_approaching       2.0
+smoothing_time_walking           0.5
+com_x_limits                     (-0.05, 0.05)
+com_y_limits                     (0.0, 0.0)

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

@@ -52,7 +52,9 @@ sampling_time           0.01
 use_joint_retargeting    1
 # enable the virtualizer
 use_virtualizer         1
-use_com_retargeting     0
+use_com_height_retargeting     0
+use_com_horizontal_retargeting 1
+
 # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link
 height_reference_frame  root_link
 

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

@@ -225,11 +225,14 @@ namespace WalkingControllers
          * @param measuredTransform transformation between the world and the (stance/swing??) foot;
          * @param mergePoint is the instant at which the old and the new trajectory will be merged;
          * @param plannerDesiredInput The desired input to the planner.
+         * @param leftZMPOffsetDelta the offset added to the delta ZMP on the left foot
+         * @param rightZMPOffsetDelta the offset added to the delta ZMP on the right foot
          * @return true/false in case of success/failure.
          */
         bool askNewTrajectories(const double& initTime, const bool& isLeftSwinging,
                                 const iDynTree::Transform& measuredTransform,
-                                const size_t& mergePoint, const iDynTree::VectorDynSize &plannerDesiredInput);
+                                const size_t& mergePoint, const iDynTree::VectorDynSize &plannerDesiredInput,
+                                const iDynTree::Vector2& leftZMPOffsetDelta, const iDynTree::Vector2& rightZMPOffsetDelta);
 
         /**
          * Update the old trajectory.

src/WalkingModule/src/Module.cpp

@@ -745,7 +745,6 @@ bool WalkingModule::updateModule()
             // when we are near to the merge point the new trajectory is evaluated
             if(m_newTrajectoryMergeCounter == m_plannerAdvanceTimeSteps)
             {
-
                 double initTimeTrajectory;
                 initTimeTrajectory = m_time + m_newTrajectoryMergeCounter * m_dT;
 
@@ -754,9 +753,13 @@ bool WalkingModule::updateModule()
                     m_leftTrajectory[m_newTrajectoryMergeCounter];
 
                 // ask for a new trajectory
+                const iDynTree::Vector2& comHorizontal = m_retargetingClient->comHorizonal();
+                iDynTree::Vector2 zmpOffset;
+                zmpOffset(0) = comHorizontal(0);
+                zmpOffset(1) = 0;
                 if(!askNewTrajectories(initTimeTrajectory, !m_isLeftFixedFrame.front(),
                                        measuredTransform, m_newTrajectoryMergeCounter,
-                                       m_plannerInput))
+                                       m_plannerInput, zmpOffset, zmpOffset))
                 {
                     yError() << "[WalkingModule::updateModule] Unable to ask for a new trajectory.";
                     return false;
@@ -1071,6 +1074,7 @@ bool WalkingModule::updateModule()
             CoMPositionDesired[2] = m_retargetingClient->comHeight() + m_comHeightOffset;
             data.vectors["com::position::desired"].assign(CoMPositionDesired.begin(), CoMPositionDesired.begin() + CoMPositionDesired.size());
             data.vectors["com::position::desired_macumba"].assign(desiredCoMPosition.data(), desiredCoMPosition.data() + desiredCoMPosition.size());
+            data.vectors["com::position::desired_retargeting"].assign(m_retargetingClient->comHorizonal().data(), m_retargetingClient->comHorizonal().data() + m_retargetingClient->comHorizonal().size());
 
             // Manual definition of this value to add also the planned CoM height velocity
             std::vector<double> CoMVelocityDesired(3);
@@ -1440,7 +1444,8 @@ bool WalkingModule::generateFirstTrajectories()
 
 bool WalkingModule::askNewTrajectories(const double& initTime, const bool& isLeftSwinging,
                                        const iDynTree::Transform& measuredTransform,
-                                       const size_t& mergePoint, const iDynTree::VectorDynSize &plannerDesiredInput)
+                                       const size_t& mergePoint, const iDynTree::VectorDynSize &plannerDesiredInput,
+                                       const iDynTree::Vector2& leftZMPOffsetDelta, const iDynTree::Vector2& rightZMPOffsetDelta)
 {
     if(m_trajectoryGenerator == nullptr)
     {
@@ -1472,7 +1477,8 @@ bool WalkingModule::askNewTrajectories(const double& initTime, const bool& isLef
 
     if(!m_trajectoryGenerator->updateTrajectories(initTime, m_DCMPositionDesired[mergePoint],
                                                   m_DCMVelocityDesired[mergePoint], isLeftSwinging,
-                                                  measuredTransform, plannerDesiredInput))
+                                                  measuredTransform, plannerDesiredInput,
+                                                  leftZMPOffsetDelta, rightZMPOffsetDelta))
     {
         yError() << "[WalkingModule::askNewTrajectories] Unable to update the trajectory.";
         return false;