Attempt to minimize the angular momentum on the z component

src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/plannerParams.ini

@@ -21,7 +21,7 @@ saturationFactors       (0.7, 0.7)
 
 ##Bounds
 #Step length
-maxStepLength           0.25
+maxStepLength           0.31
 minStepLength           0.01
 maxLengthBackwardFactor 1.0
 #Width
@@ -45,11 +45,11 @@ comHeightDelta          0.01
 #Timings
 nominalDuration         1.3
 lastStepSwitchTime      0.8
-switchOverSwingRatio    0.3
+switchOverSwingRatio    0.1
 
 #ZMP Delta
-leftZMPDelta           (0.005 -0.0)
-rightZMPDelta          (0.005 0.0)
+leftZMPDelta           (0.0 -0.0)
+rightZMPDelta          (0.0 0.0)
 
 #Feet cartesian offset on the yaw
 leftYawDeltaInDeg      0.0

src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/inverseKinematics.ini

@@ -26,8 +26,8 @@ max-cpu-time            20
 
 #DEGREES
 jointRegularization     (7, 0.12, -0.01,
-                         -3.7, 20.0, -13.0, 40.769,
-                         -3.7, 20.0, -13.0, 40.769,
+                         0.0, 0.0, -0.0, 0.0,
+                         0.0, 0.0, -0.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)
 

src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini

@@ -24,5 +24,10 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[ANGULAR_MOMENTUM_TASK]
+robot_velocity_variable_name    "robot_velocity"
+mask                            (false, false, true)
+weight                          (2.0)
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]

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

@@ -1,26 +1,26 @@
 robot_velocity_variable_name    "robot_velocity"
-kp                              (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)
+kp                              (5.0, 5.0, 1.0,
+                                 0.5, 5.0, 5.0, 0.5,
+                                 0.5, 5.0, 5.0, 0.5,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
 
 states          ("stance", "walking")
 sampling_time   0.002
 settling_time   0.5
 
 [stance]
 name    "stance"
-weight  (1.0, 1.0, 1.0,
-         2.0, 2.0, 2.0, 2.0,
-         2.0, 2.0, 2.0, 2.0,
+weight  (1.0, 1.0, 0.5,
+         1.0, 2.0, 2.0, 1.0,
+         1.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, 1.0)
 
 [walking]
 name    "walking"
-weight  (1.0, 1.0, 1.0,
-         2.0, 2.0, 2.0, 2.0,
-         2.0, 2.0, 2.0, 2.0,
+weight  (1.0, 1.0, 0.5,
+         1.0, 2.0, 2.0, 1.0,
+         1.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, 1.0)

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

@@ -1,6 +1,6 @@
 robot_velocity_variable_name    "robot_velocity"
 frame_name                      "chest"
-kp_angular                      5.0
+kp_angular                      50.0
 
 
 states          ("stance", "walking")
@@ -9,8 +9,8 @@ settling_time   0.5
 
 [stance]
 name    "stance"
-weight  (5.0, 5.0, 5.0)
+weight  (15.0, 15.0, 1.0)
 
 [walking]
 name    "walking"
-weight  (5.0, 5.0, 5.0)
+weight  (15.0, 15.0, 1.0)

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

@@ -17,7 +17,7 @@ skip_dcm_controller                  1
 goal_port_scaling                   (0.5, 1.0, 0.5)
 
 # How much in advance the planner should be called. The time is in seconds
-planner_advance_time_in_s           0.02
+planner_advance_time_in_s           0.08
 
 # How much time (in seconds) before failing due to missing feedback
 max_feedback_delay_in_s             1.0
@@ -29,7 +29,7 @@ remove_zmp_offset                   0
 [GENERAL]
 name                    walking-coordinator
 # height of the com
-com_height              0.7
+com_height              0.715
 # sampling time
 sampling_time           0.01
 # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link

src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/inverseKinematics.ini

@@ -32,8 +32,8 @@ max-cpu-time            20
 #                          5.76, 1.61, -0.31, -31.64, -20.52, -1.52)
 
 jointRegularization     (7, 0.12, -0.01,
-                         12.0, 7.0, -12.0, 40.769,
-                         12.0, 7.0, -12.0, 40.769,
+                         0.0, 0.0, 0.0, 0.0
+                         0.0, 0.0, 0.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)
 

src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini

@@ -24,5 +24,10 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[ANGULAR_MOMENTUM_TASK]
+robot_velocity_variable_name    "robot_velocity"
+mask                            (false, false, true)
+weight                          (5.0)
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]

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

@@ -1,7 +1,7 @@
 robot_velocity_variable_name    "robot_velocity"
-kp                              (5.0, 5.0, 5.0,
-                                 5.0, 5.0, 5.0, 5.0,
-                                 5.0, 5.0, 5.0, 5.0,
+kp                              (5.0, 5.0, 1.0,
+                                 0.5, 5.0, 5.0, 0.5,
+                                 0.5, 5.0, 5.0, 0.5,
                                  5.0, 5.0, 5.0, 5.0, 5.0, 5.0,
                                  5.0, 5.0, 5.0, 5.0, 5.0, 5.0)
 
@@ -11,16 +11,16 @@ settling_time   0.5
 
 [stance]
 name    "stance"
-weight  (1.0, 1.0, 1.0,
-         2.0, 2.0, 2.0, 2.0,
-         2.0, 2.0, 2.0, 2.0,
+weight  (1.0, 1.0, 0.5,
+         1.0, 2.0, 2.0, 1.0,
+         1.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, 1.0)
 
 [walking]
 name    "walking"
-weight  (1.0, 1.0, 1.0,
-         2.0, 2.0, 2.0, 2.0,
-         2.0, 2.0, 2.0, 2.0,
+weight  (1.0, 1.0, 0.5,
+         1.0, 2.0, 2.0, 1.0,
+         1.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, 1.0)

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

@@ -1,6 +1,6 @@
 robot_velocity_variable_name    "robot_velocity"
 frame_name                      "chest"
-kp_angular                      5.0
+kp_angular                      10.0
 
 
 states          ("stance", "walking")
@@ -9,8 +9,8 @@ settling_time   0.5
 
 [stance]
 name    "stance"
-weight  (5.0, 5.0, 5.0)
+weight  (10.0, 10.0, 10.0)
 
 [walking]
 name    "walking"
-weight  (5.0, 5.0, 5.0)
+weight  (10.0, 10.0, 10.0)

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

@@ -184,6 +184,7 @@ namespace WalkingControllers
         bool solveBLFIK(const iDynTree::Position& desiredCoMPosition,
                         const iDynTree::Vector3& desiredCoMVelocity,
                         const iDynTree::Rotation& desiredNeckOrientation,
+                        const iDynTree::SpatialMomentum& desiredCentroidalMomentum,
                         iDynTree::VectorDynSize &output);
 
         /**
@@ -199,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

@@ -371,6 +371,9 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf)
             return false;
         }
 
+        m_vectorsCollectionServer.populateMetadata("angular_momentum::measured", {"x", "y", "z"});
+        m_vectorsCollectionServer.populateMetadata("angular_momentum::desired", {"x", "y", "z"});
+
         m_vectorsCollectionServer.populateMetadata("dcm::position::measured", {"x", "y"});
         m_vectorsCollectionServer.populateMetadata("dcm::position::desired", {"x", "y"});
         m_vectorsCollectionServer.populateMetadata("dcm::velocity::desired", {"x", "y"});
@@ -502,6 +505,7 @@ bool WalkingModule::close()
 bool WalkingModule::solveBLFIK(const iDynTree::Position &desiredCoMPosition,
                                const iDynTree::Vector3 &desiredCoMVelocity,
                                const iDynTree::Rotation &desiredNeckOrientation,
+                               const iDynTree::SpatialMomentum &centroidalMomentumDesired,
                                iDynTree::VectorDynSize &output)
 {
     const std::string phase = m_isStancePhase.front() ? "stance" : "walking";
@@ -516,6 +520,8 @@ bool WalkingModule::solveBLFIK(const iDynTree::Position &desiredCoMPosition,
     ok = ok && m_BLFIKSolver->setRetargetingJointSetPoint(m_retargetingClient->jointPositions(),
                                                           m_retargetingClient->jointVelocities());
 
+    ok = ok && m_BLFIKSolver->setAngularMomentumSetPoint(centroidalMomentumDesired.getAngularVec3());
+
     if (m_useRootLinkForHeight)
     {
         ok = ok && m_BLFIKSolver->setRootSetPoint(desiredCoMPosition, desiredCoMVelocity);
@@ -902,6 +908,10 @@ bool WalkingModule::updateModule()
         yawRotation = yawRotation.inverse();
         modifiedInertial = yawRotation * m_inertial_R_worldFrame;
 
+        // compute the desired torso velocity
+        const iDynTree::Twist desiredTorsoVelocity = this->computeAverageTwistFromPlannedFeet();
+        auto centroidalMomentumDesired = m_FKSolver->getKinDyn()->getCentroidalRobotLockedInertia() * desiredTorsoVelocity;
+
         if (m_useQPIK)
         {
             // integrate dq because velocity control mode seems not available
@@ -919,6 +929,7 @@ bool WalkingModule::updateModule()
             if (!solveBLFIK(desiredCoMPosition,
                             desiredCoMVelocity,
                             yawRotation,
+                            centroidalMomentumDesired,
                             m_dqDesired))
             {
                 yError() << "[WalkingModule::updateModule] Unable to solve the QP problem with "
@@ -1023,6 +1034,8 @@ bool WalkingModule::updateModule()
             CoMVelocityDesired[1] = m_stableDCMModel->getCoMVelocity().data()[1];
             CoMVelocityDesired[2] = m_retargetingClient->comHeightVelocity();
 
+            m_vectorsCollectionServer.prepareData();
+            m_vectorsCollectionServer.clearData();
             m_vectorsCollectionServer.populateData("com::velocity::desired", CoMVelocityDesired);
 
             // Left foot position
@@ -1083,6 +1096,9 @@ bool WalkingModule::updateModule()
             const double isLeftFootFixed = m_isLeftFixedFrame.front() ? 1.0 : 0.0;
             m_vectorsCollectionServer.populateData("stance_foot::is_left", std::array<double, 1>{isLeftFootFixed});
 
+            m_vectorsCollectionServer.populateData("angular_momentum::measured", m_FKSolver->getKinDyn()->getCentroidalTotalMomentum().getAngularVec3());
+            m_vectorsCollectionServer.populateData("angular_momentum::desired", centroidalMomentumDesired.getAngularVec3());            
+
             m_vectorsCollectionServer.sendData();
         }
 
@@ -1124,6 +1140,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)

src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h

@@ -18,6 +18,7 @@
 #include <BipedalLocomotion/IK/R3Task.h>
 #include <BipedalLocomotion/IK/SE3Task.h>
 #include <BipedalLocomotion/IK/SO3Task.h>
+#include <BipedalLocomotion/IK/AngularMomentumTask.h>
 #include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
 #include <BipedalLocomotion/System/VariablesHandler.h>
 
@@ -48,8 +49,11 @@ class BLFIK
     bool setCoMSetPoint(const iDynTree::Position& position, const iDynTree::Vector3& velocity);
     bool setRootSetPoint(const iDynTree::Position& position, const iDynTree::Vector3& velocity);
     bool setTorsoSetPoint(const iDynTree::Rotation& rotation);
+    bool setAngularMomentumSetPoint(const iDynTree::Vector3& angularMomentum);
     const iDynTree::VectorDynSize& getDesiredJointVelocity() const;
 
+    iDynTree::Twist getDesiredTorsoVelocity() const;
+
 private:
     std::shared_ptr<BipedalLocomotion::ContinuousDynamicalSystem::MultiStateWeightProvider>
         m_torsoWeight;
@@ -68,6 +72,7 @@ class BLFIK
     std::shared_ptr<BipedalLocomotion::IK::R3Task> m_rootTask;
     std::shared_ptr<BipedalLocomotion::IK::JointTrackingTask> m_jointRetargetingTask;
     std::shared_ptr<BipedalLocomotion::IK::JointTrackingTask> m_jointRegularizationTask;
+    std::shared_ptr<BipedalLocomotion::IK::AngularMomentumTask> m_angularMomentumTask;
 
     iDynTree::VectorDynSize m_jointVelocity;
     bool m_usejointRetargeting{false};

src/WholeBodyControllers/src/BLFIK.cpp

@@ -102,6 +102,24 @@ bool BLFIK::initialize(
                            lowPriority,
                            m_jointRegularizationWeight);
 
+    m_angularMomentumTask = std::make_shared<BipedalLocomotion::IK::AngularMomentumTask>();
+    ok = ok && m_angularMomentumTask->setKinDyn(kinDyn);
+    ok = ok && m_angularMomentumTask->initialize(ptr->getGroup("ANGULAR_MOMENTUM_TASK"));
+
+    Eigen::VectorXd angularMomentumWeight;
+    ok = ok && ptr->getGroup("ANGULAR_MOMENTUM_TASK").lock()->getParameter("weight", angularMomentumWeight);
+
+    ok = ok
+         && m_qpIK.addTask(m_angularMomentumTask,
+                           "angular_momentum_task",
+                           lowPriority,
+                           angularMomentumWeight);
+    if (!ok)
+    {
+        BipedalLocomotion::log()->error("{} Unable to initialize the angular momentum task.", prefix);
+        return false;
+    }
+
     if (m_usejointRetargeting)
     {
         m_jointRetargetingTask = std::make_shared<BipedalLocomotion::IK::JointTrackingTask>();
@@ -200,6 +218,11 @@ bool BLFIK::setRegularizationJointSetPoint(const iDynTree::VectorDynSize& jointP
     return m_jointRegularizationTask->setSetPoint(iDynTree::toEigen(jointPosition));
 }
 
+bool BLFIK::setAngularMomentumSetPoint(const iDynTree::Vector3& angularMomentum)
+{
+    return m_angularMomentumTask->setSetPoint(iDynTree::toEigen(angularMomentum));
+}
+
 bool BLFIK::setCoMSetPoint(const iDynTree::Position& position, const iDynTree::Vector3& velocity)
 {
     return m_comTask->setSetPoint(iDynTree::toEigen(position), iDynTree::toEigen(velocity));
@@ -223,3 +246,11 @@ const iDynTree::VectorDynSize& BLFIK::getDesiredJointVelocity() const
 {
     return m_jointVelocity;
 }
+
+iDynTree::Twist BLFIK::getDesiredTorsoVelocity() const
+{
+    iDynTree::Twist tmp;
+    tmp.zero();
+    iDynTree::toEigen(tmp.getAngularVec3()) = m_torsoTask->getB();
+    return tmp;
+}