Use the average of the feet velocity as desired centroidal locked velocity

Author: GiulioRomualdi

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

@@ -200,6 +200,8 @@ namespace WalkingControllers
          */
         iDynTree::Rotation computeAverageYawRotationFromPlannedFeet() const;
 
+        iDynTree::Twist computeAverageTwistFromPlannedFeet() const;
+
         /**
          * Generate the first trajectory.
          * This method has to be called before updateTrajectories() method.

src/WalkingModule/src/Module.cpp

@@ -903,8 +903,8 @@ bool WalkingModule::updateModule()
         modifiedInertial = yawRotation * m_inertial_R_worldFrame;
 
         // compute the desired torso velocity
-        auto torsoVelocity = m_BLFIKSolver->getDesiredTorsoVelocity();
-        auto centroidalMomentumDesired = m_FKSolver->getKinDyn()->getCentroidalRobotLockedInertia() * torsoVelocity;
+        const iDynTree::Twist desiredTorsoVelocity = this->computeAverageTwistFromPlannedFeet();
+        auto centroidalMomentumDesired = m_FKSolver->getKinDyn()->getCentroidalRobotLockedInertia() * desiredTorsoVelocity;
 
         if (m_useQPIK)
         {
@@ -1127,6 +1127,21 @@ iDynTree::Rotation WalkingModule::computeAverageYawRotationFromPlannedFeet() con
     return iDynTree::Rotation::RotZ(meanYaw);
 }
 
+iDynTree::Twist WalkingModule::computeAverageTwistFromPlannedFeet() const
+{
+    iDynTree::Twist twist;
+    iDynTree::Vector3 meanLinearVelocity, meanAngularVelocity;
+    iDynTree::toEigen(meanLinearVelocity) = (iDynTree::toEigen(m_leftTwistTrajectory.front().getLinearVec3()) +
+                                            iDynTree::toEigen(m_rightTwistTrajectory.front().getLinearVec3())) / 2.0;
+    iDynTree::toEigen(meanAngularVelocity) = (iDynTree::toEigen(m_leftTwistTrajectory.front().getAngularVec3()) +
+                                          iDynTree::toEigen(m_rightTwistTrajectory.front().getAngularVec3())) / 2.0;
+
+    twist.setLinearVec3(meanLinearVelocity);
+    twist.setAngularVec3(meanAngularVelocity);
+
+    return twist;
+}
+
 bool WalkingModule::prepareRobot(bool onTheFly)
 {
     if (m_robotState != WalkingFSM::Configured && m_robotState != WalkingFSM::Stopped)