Attempt to minimize the angular momentum on the z component

Author: GiulioRomualdi

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.34
 minStepLength           0.01
 maxLengthBackwardFactor 1.0
 #Width
@@ -32,7 +32,7 @@ maxAngleVariation       18.0
 minAngleVariation       5.0
 #Timings
 maxStepDuration         1.31
-minStepDuration         0.7
+minStepDuration         0.5
 
 ##Nominal Values
 #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/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>
 
@@ -68,6 +69,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>();
@@ -124,6 +142,9 @@ bool BLFIK::initialize(
 
     ok = ok && m_qpIK.finalize(m_variableHandler);
 
+    m_angularMomentumTask->setSetPoint(Eigen::Vector3d::Zero());
+
+
     BipedalLocomotion::log()->info("[BLFIK::initialize] {}", m_qpIK.toString());
 
     m_jointVelocity.resize(kinDyn->getNrOfDegreesOfFreedom());