Reduce start delay

CMakeLists.txt

@@ -6,7 +6,7 @@ cmake_minimum_required(VERSION 3.5)
 
 ## MAIN project
 project(WalkingControllers
-  VERSION 0.6.1)
+  VERSION 0.6.2)
 
 # Defines the CMAKE_INSTALL_LIBDIR, CMAKE_INSTALL_BINDIR and many other useful macros.
 # See https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html

cmake/WalkingControllersDependencies.cmake

@@ -7,7 +7,7 @@ find_package(ICUB REQUIRED)
 find_package(ICUBcontrib REQUIRED)
 find_package(iDynTree REQUIRED)
 find_package(Eigen3 3.2.92 REQUIRED)
-find_package(UnicyclePlanner 0.5.0 REQUIRED)
+find_package(UnicyclePlanner 0.5.1 REQUIRED)
 find_package(OsqpEigen 0.4.0 REQUIRED)
 find_package(qpOASES REQUIRED)
 find_package(BipedalLocomotionFramework 0.9.0

src/TrajectoryPlanner/src/TrajectoryGenerator.cpp

@@ -127,7 +127,6 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config)
     double lastStepSwitchTime = config.check("lastStepSwitchTime", yarp::os::Value(0.5)).asFloat64();
     double switchOverSwingRatio = config.check("switchOverSwingRatio",
                                                yarp::os::Value(0.4)).asFloat64();
-    double mergePointRatio = config.check("mergePointRatio", yarp::os::Value(0.5)).asFloat64();
     double lastStepDCMOffset = config.check("lastStepDCMOffset", yarp::os::Value(0.0)).asFloat64();
     m_leftYawDeltaInRad = iDynTree::deg2rad(config.check("leftYawDeltaInDeg", yarp::os::Value(0.0)).asFloat64());
     m_rightYawDeltaInRad = iDynTree::deg2rad(config.check("rightYawDeltaInDeg", yarp::os::Value(0.0)).asFloat64());
@@ -140,6 +139,13 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config)
                                     yarp::os::Value(false)).asBool();
     double pitchDelta = config.check("pitchDelta", yarp::os::Value(0.0)).asFloat64();
 
+    iDynTree::Vector2 mergePointRatios;
+    if(!YarpUtilities::getVectorFromSearchable(config, "mergePointRatios", mergePointRatios))
+    {
+        yError() << "[configurePlanner] Initialization failed while reading mergePointRatios vector.";
+        return false;
+    }
+
     yarp::os::Bottle ellipseMethodGroup = config.findGroup("ELLIPSE_METHOD_SETTINGS");
     double freeSpaceConservativeFactor = ellipseMethodGroup.check("conservative_factor", yarp::os::Value(2.0)).asFloat64();
     double innerEllipseSemiMajorOffset = ellipseMethodGroup.check("inner_offset_major", yarp::os::Value(0.0)).asFloat64();
@@ -190,7 +196,7 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config)
 
     m_heightGenerator = m_trajectoryGenerator.addCoMHeightTrajectoryGenerator();
     ok = ok && m_heightGenerator->setCoMHeightSettings(comHeight, comHeightDelta);
-    ok = ok && m_trajectoryGenerator.setMergePointRatio(mergePointRatio);
+    ok = ok && m_trajectoryGenerator.setMergePointRatio(mergePointRatios[0], mergePointRatios[1]);
 
     m_dcmGenerator = m_trajectoryGenerator.addDCMTrajectoryGenerator();
     m_dcmGenerator->setFootOriginOffset(leftZMPDelta, rightZMPDelta);

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

@@ -61,7 +61,10 @@ rightYawDeltaInDeg     0.0
 lastStepDCMOffset      0.25
 
 #MergePoint
-mergePointRatio         0.4
+# The ratios of the double support in which it is present a merge point.
+# The first number is the ratio at which the merge points begin, the second
+# when they ends.
+mergePointRatios       (0.4, 0.4)
 
 # pitch delta
 pitchDelta		0.0

src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_hand_retargeting.ini

@@ -36,6 +36,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 goal_port_scaling                   (10.0, 10.0, 1.0)
 
+# How much in advance the planner should be called. The time is in seconds
+planner_advance_time_in_s           0.18
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini

@@ -38,6 +38,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 goal_port_scaling                   (10.0, 10.0, 1.0)
 
+# How much in advance the planner should be called. The time is in seconds
+planner_advance_time_in_s           0.18
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

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

@@ -36,6 +36,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 goal_port_scaling                   (10.0, 10.0, 1.0)
 
+# How much in advance the planner should be called. The time is in seconds
+planner_advance_time_in_s           0.18
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

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

@@ -61,7 +61,10 @@ rightYawDeltaInDeg     0.0
 lastStepDCMOffset      0.25
 
 #MergePoint
-mergePointRatio         0.4
+# The ratios of the double support in which it is present a merge point.
+# The first number is the ratio at which the merge points begin, the second
+# when they ends.
+mergePointRatios       (0.4, 0.8)
 
 # pitch delta
 pitchDelta		0.0

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_hand_retargeting.ini

@@ -36,6 +36,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 goal_port_scaling                   (10.0, 10.0, 1.0)
 
+# How much in advance the planner should be called. The time is in seconds
+planner_advance_time_in_s           0.18
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_joint_retargeting.ini

@@ -36,6 +36,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 goal_port_scaling                   (10.0, 10.0, 1.0)
 
+# How much in advance the planner should be called. The time is in seconds
+planner_advance_time_in_s           0.18
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_retargeting.ini

@@ -36,6 +36,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 goal_port_scaling                   (10.0, 10.0, 1.0)
 
+# How much in advance the planner should be called. The time is in seconds
+planner_advance_time_in_s           0.18
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

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

@@ -38,6 +38,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 goal_port_scaling                   (1.0, 1.0, 1.0)
 
+# How much in advance the planner should be called. The time is in seconds
+planner_advance_time_in_s           0.18
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/plannerParams.ini

@@ -61,7 +61,10 @@ rightYawDeltaInDeg     0.0
 lastStepDCMOffset      0.25
 
 #MergePoint
-mergePointRatio         0.4
+# The ratios of the double support in which it is present a merge point.
+# The first number is the ratio at which the merge points begin, the second
+# when they ends.
+mergePointRatios       (0.4, 0.4)
 
 # pitch delta
 # pitchDelta            -3.0

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

@@ -55,7 +55,10 @@ leftYawDeltaInDeg      0.0
 rightYawDeltaInDeg     0.0
 
 #MergePoint
-mergePointRatio         0.4
+# The ratios of the double support in which it is present a merge point.
+# The first number is the ratio at which the merge points begin, the second
+# when they ends.
+mergePointRatios       (0.4, 0.4)
 
 # Last Step DCM Offset
 # If it is 0.5 the final DCM will be in the middle of the two footsteps;

src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini

@@ -55,7 +55,10 @@ leftYawDeltaInDeg      0.0
 rightYawDeltaInDeg     0.0
 
 #MergePoint
-mergePointRatio         0.4
+# The ratios of the double support in which it is present a merge point.
+# The first number is the ratio at which the merge points begin, the second
+# when they ends.
+mergePointRatios       (0.4, 0.4)
 
 # Last Step DCM Offset
 # If it is 0.5 the final DCM will be in the middle of the two footsteps;

src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/plannerParams.ini

@@ -62,7 +62,10 @@ lastStepDCMOffset      0.25
 
 
 #MergePoint
-mergePointRatio         0.4
+# The ratios of the double support in which it is present a merge point.
+# The first number is the ratio at which the merge points begin, the second
+# when they ends.
+mergePointRatios       (0.4, 0.4)
 
 # pitch delta
 # pitchDelta            -3.0

src/WalkingModule/app/robots/iCubGenova04/dcm_walking_hand_retargeting.ini

@@ -36,6 +36,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 goal_port_scaling                   (10.0, 10.0, 1.0)
 
+# How much in advance the planner should be called. The time is in seconds
+planner_advance_time_in_s           0.18
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

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

@@ -20,6 +20,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 goal_port_scaling                   (10.0, 10.0, 1.0)
 
+# How much in advance the planner should be called. The time is in seconds
+planner_advance_time_in_s           0.18
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini

@@ -36,6 +36,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 goal_port_scaling                   (10.0, 10.0, 1.0)
 
+# How much in advance the planner should be called. The time is in seconds
+planner_advance_time_in_s           0.18
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

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

@@ -36,6 +36,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 goal_port_scaling                   (10.0, 10.0, 1.0)
 
+# How much in advance the planner should be called. The time is in seconds
+planner_advance_time_in_s           0.18
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

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

@@ -61,7 +61,10 @@ rightYawDeltaInDeg     0.0
 lastStepDCMOffset      0.25
 
 #MergePoint
-mergePointRatio         0.4
+# The ratios of the double support in which it is present a merge point.
+# The first number is the ratio at which the merge points begin, the second
+# when they ends.
+mergePointRatios       (0.4, 0.4)
 
 # pitch delta
 pitchDelta		0.0

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

@@ -36,6 +36,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 goal_port_scaling                   (10.0, 10.0, 1.0)
 
+# How much in advance the planner should be called. The time is in seconds
+planner_advance_time_in_s           0.18
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

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

@@ -38,6 +38,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 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.08
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

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

@@ -36,6 +36,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 goal_port_scaling                   (10.0, 10.0, 1.0)
 
+# How much in advance the planner should be called. The time is in seconds
+planner_advance_time_in_s           0.18
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

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

@@ -38,6 +38,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 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.08
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

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

@@ -60,7 +60,10 @@ rightYawDeltaInDeg     0.0
 lastStepDCMOffset      0.25
 
 #MergePoint
-mergePointRatio         0.5
+# The ratios of the double support in which it is present a merge point.
+# The first number is the ratio at which the merge points begin, the second
+# when they ends.
+mergePointRatios       (0.5, 0.5)
 
 #Should be the first step with the left foot?
 swingLeft               1

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

@@ -36,6 +36,9 @@ goal_port_suffix                    /goal:i
 # Scales the input coming from the goal port
 goal_port_scaling                   (10.0, 10.0, 1.0)
 
+# How much in advance the planner should be called. The time is in seconds
+planner_advance_time_in_s           0.18
+
 # general parameters
 [GENERAL]
 name                    walking-coordinator

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

@@ -146,6 +146,8 @@ namespace WalkingControllers
 
         iDynTree::VectorDynSize m_plannerInput, m_goalScaling;
 
+        size_t m_plannerAdvanceTimeSteps; /** How many steps in advance the planner should be called. */
+
         // debug
         std::unique_ptr<iCub::ctrl::Integrator> m_velocityIntegral{nullptr};
 

src/WalkingModule/src/Module.cpp

@@ -9,6 +9,7 @@
 // std
 #include <iostream>
 #include <memory>
+#include <algorithm>
 
 // YARP
 #include <yarp/os/RFModule.h>
@@ -185,7 +186,17 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf)
     }
 
     yarp::os::Bottle& generalOptions = rf.findGroup("GENERAL");
-    m_dT = generalOptions.check("sampling_time", yarp::os::Value(0.016)).asFloat64();
+    m_dT = generalOptions.check("sampling_time", yarp::os::Value(0.01)).asFloat64();
+
+    if (m_dT <= 0)
+    {
+        yError() << "[WalkingModule::configure] sampling_time is supposed to be strictly positive.";
+        return false;
+    }
+
+    double plannerAdvanceTimeInS = rf.check("planner_advance_time_in_s", yarp::os::Value(0.18)).asFloat64();
+    m_plannerAdvanceTimeSteps = std::round(plannerAdvanceTimeInS / m_dT) + 2; //The additional 2 steps are because the trajectory from the planner is requested two steps in advance wrt the merge point
+
     std::string name;
     if(!YarpUtilities::getStringFromSearchable(generalOptions, "name", name))
     {
@@ -732,7 +743,7 @@ bool WalkingModule::updateModule()
         if(m_newTrajectoryRequired)
         {
             // when we are near to the merge point the new trajectory is evaluated
-            if(m_newTrajectoryMergeCounter == 20)
+            if(m_newTrajectoryMergeCounter == m_plannerAdvanceTimeSteps)
             {
 
                 double initTimeTrajectory;
@@ -1641,27 +1652,28 @@ bool WalkingModule::setPlannerInput(const yarp::sig::Vector &plannerInput)
             return true;
 
         // Since the evaluation of a new trajectory takes time the new trajectory will be merged after x cycles
-        m_newTrajectoryMergeCounter = 20;
+        m_newTrajectoryMergeCounter = m_plannerAdvanceTimeSteps;
     }
 
     // the trajectory was not finished the new trajectory will be attached at the next merge point
     else
     {
-        if(m_mergePoints.front() > 20)
-            m_newTrajectoryMergeCounter = m_mergePoints.front();
-        else if(m_mergePoints.size() > 1)
+        //Searches for the first merge point that is at least m_plannerAdvanceTimeSteps steps away
+        auto firstMergePointAvailable = std::find_if(m_mergePoints.begin(), m_mergePoints.end(), [this](size_t input){return input >= m_plannerAdvanceTimeSteps;});
+
+        if(firstMergePointAvailable != m_mergePoints.end())
         {
             if(m_newTrajectoryRequired)
                 return true;
 
-            m_newTrajectoryMergeCounter = m_mergePoints[1];
+            m_newTrajectoryMergeCounter = *firstMergePointAvailable;
         }
         else
         {
             if(m_newTrajectoryRequired)
                 return true;
 
-            m_newTrajectoryMergeCounter = 20;
+            m_newTrajectoryMergeCounter = m_plannerAdvanceTimeSteps;
         }
     }