Merge pull request #64 from robotology/fix_restart_issue_part2

Attempt to reduce jumping issues when the pause conditions get active.

.github/workflows/conda-forge-ci.yml

@@ -5,7 +5,7 @@ on:
   pull_request:
 #  schedule:
   # * is a special character in YAML so you have to quote this string
-  # Execute a "nightly" build at 2 AM UTC 
+  # Execute a "nightly" build at 2 AM UTC
 #  - cron:  '0 2 * * *'
 
 jobs:
@@ -23,26 +23,25 @@ jobs:
 
     - uses: conda-incubator/setup-miniconda@v2
       with:
-        mamba-version: "*"
-        channels: conda-forge,defaults
-        channel-priority: true
+        miniforge-variant: Mambaforge
+        miniforge-version: latest
 
     - name: Dependencies
       shell: bash -l {0}
       run: |
-        # Compilation related dependencies 
+        # Compilation related dependencies
         mamba install cmake compilers make ninja pkg-config
         # Actual dependencies
         mamba install eigen idyntree
-        
+
     - name: Configure [Linux&macOS]
       if: contains(matrix.os, 'macos') || contains(matrix.os, 'ubuntu')
       shell: bash -l {0}
       run: |
         mkdir -p build
         cd build
         cmake -GNinja -DBUILD_TESTING:BOOL=ON -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} ..
-      
+
     - name: Configure [Windows]
       if: contains(matrix.os, 'windows')
       shell: bash -l {0}
@@ -51,12 +50,12 @@ jobs:
         cd build
         cmake -G"Visual Studio 16 2019" -DBUILD_TESTING:BOOL=ON -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} ..
 
-    - name: Build 
+    - name: Build
       shell: bash -l {0}
       run: |
         cd build
         cmake --build . --config ${{ matrix.build_type }}
-        
+
     - name: Test
       shell: bash -l {0}
       run: |

include/DCMTrajectoryGenerator.h

@@ -46,7 +46,7 @@ class DCMTrajectoryGenerator {
      * @param right Right Steps
      * @return True in case of success.
      */
-    bool computeNewTrajectories(double initTime, double dT, double switchPercentage, double maxStepTime,
+    bool computeNewTrajectories(double initTime, double dT, double switchPercentage, double maxStepTime, double endSwitchTime,
                                 double nominalStepTime, bool pauseActive, const std::vector<const Step *> &orderedSteps,
                                 const std::vector<size_t> &phaseShift, const std::vector<StepPhase> &lFootPhases,
                                 const FootPrint &left, const FootPrint &right);

include/DCMTrajectoryGeneratorHelper.h

@@ -142,6 +142,7 @@ class DCMTrajectoryGeneratorHelper
 
     double m_maxDoubleSupportDuration; /**< Max duration of a DS phase. */
     double m_nominalDoubleSupportDuration; /**< Nominal duration of a DS phase. */
+    double m_endSwitchTime; /**< Duration of the switch time to get to a stop. */
     bool m_pauseActive; /**< True if the pause feature is activate. */
 
     std::vector<std::shared_ptr<GeneralSupportTrajectory>> m_trajectory; /**< Vector containing pointer of every trajectory phase. */
@@ -152,6 +153,8 @@ class DCMTrajectoryGeneratorHelper
     std::vector<iDynTree::Vector2> m_ZMPPosition; /**< Vector containing the position of the ZMP. */
     std::vector<double> m_weightInLeft, m_weightInRight; /**< Vectors containing the percentage of weight carried by the foot. */
 
+    bool m_timingWarningPrinted{false}; /**< Flag used to print a warning message about the timings only once. */
+
 
     /**
      * Add the last Single and Double support phases.
@@ -279,11 +282,14 @@ class DCMTrajectoryGeneratorHelper
 
     /**
      * Set the pause condition.
+     * @param pauseActive is a flag used to activate the pause feature;
      * @param maxDoubleSupportDuration is the maximum duration of a DS phase;
      * @param nominalDoubleSupportDuration is the nominal duration of a DS phase.
+     * @param endSwitchTime is the duration of the switch time to get to a stop.
      * @return true if the pause conditions are set, false otherwise.
      */
-    bool setPauseConditions(bool pauseActive, const double &maxDoubleSupportDuration, const double &nominalDoubleSupportDuration);
+    bool setPauseConditions(bool pauseActive, const double &maxDoubleSupportDuration,
+                            const double &nominalDoubleSupportDuration, const double& endSwitchTime);
 
     /**
      * Set the last step DCM offset

include/UnicycleGenerator.h

@@ -51,14 +51,15 @@ class UnicycleGenerator {
     //Settings
     bool setSwitchOverSwingRatio(double ratio = 1.0); //indeed the swing time cannot be null, while the switch time can be very close to zero (but not zero)
 
-    bool setTerminalHalfSwitchTime(double lastHalfSwitchTime); //if not set, it won't bring the ZMP at the center of the feet at the end
+    bool setTerminalHalfSwitchTime(double lastHalfSwitchTime);
 
     bool setPauseConditions(double maxStepTime, double nominalStepTime);
 
     void setPauseActive(bool isPauseActive);
 
     void disablePauseConditions();
 
+    [[deprecated("Use unicyclePlanner()->setNavigationPath(path) instead")]]
     bool setNavigationPath(const std::vector<UnicycleState> &path);
 
     /**

include/ZMPTrajectoryGenerator.h

@@ -25,7 +25,7 @@ class ZMPTrajectoryGenerator {
     class ZMPTrajectoryGeneratorImplementation;
     std::unique_ptr<ZMPTrajectoryGeneratorImplementation> m_pimpl;
 
-    bool computeNewTrajectories(double initTime, double dT, double switchPercentage, double maxStepTime,
+    bool computeNewTrajectories(double initTime, double dT, double switchPercentage, double maxStepTime, double endSwitchTime,
                                 double nominalStepTime, bool pauseActive, const std::vector<size_t> &mergePoints,
                                 const FootPrint &left, const FootPrint &right,
                                 const std::vector<const Step*>& orderedSteps,

src/DCMTrajectoryGenerator.cpp

@@ -38,7 +38,7 @@ DCMTrajectoryGenerator::DCMTrajectoryGenerator()
 
 }
 
-bool DCMTrajectoryGenerator::computeNewTrajectories(double initTime, double dT, double switchPercentage, double maxStepTime,
+bool DCMTrajectoryGenerator::computeNewTrajectories(double initTime, double dT, double switchPercentage, double maxStepTime, double endSwitchTime,
                                                     double nominalStepTime, bool pauseActive, const std::vector<const Step *> &orderedSteps,
                                                     const std::vector<size_t> &phaseShift, const std::vector<StepPhase> &lFootPhases,
                                                     const FootPrint &left, const FootPrint &right)
@@ -91,7 +91,7 @@ bool DCMTrajectoryGenerator::computeNewTrajectories(double initTime, double dT,
     double maxSwitchTime = switchPercentage * maxStepTime;
     double nominalSwitchTime = switchPercentage * nominalStepTime;
 
-    if (!m_pimpl->helper.setPauseConditions(pauseActive, maxSwitchTime, nominalSwitchTime)) {
+    if (!m_pimpl->helper.setPauseConditions(pauseActive, maxSwitchTime, nominalSwitchTime, endSwitchTime)) {
         std::cerr << "[DCMTrajectoryGenerator::interpolateDCM] Failed to set pause conditions." << std::endl;
         return false;
     }

src/DCMTrajectoryGeneratorHelper.cpp

@@ -322,19 +322,6 @@ class DoubleSupportTrajectoryMinJerk : public GeneralSupportTrajectory
 
 public:
 
-    /**
-     * Constructor.
-     * @param initBoundaryCondition desired init position and velocity of the
-     * double support trajectory
-     * @param finalBoundaryCondition desired final position and velocity of the
-     * double support trajectory
-     * @param omega time constant of the 3D-LIPM
-     * @note the  desired initial acceleration and jerk are set to zero.
-     */
-    DoubleSupportTrajectoryMinJerk(const DCMTrajectoryPoint &initBoundaryCondition,
-                                   const DCMTrajectoryPoint &finalBoundaryCondition,
-                                   const double& omega);
-
     /**
      * Constructor.
      * @param initBoundaryCondition desired init position and velocity of the
@@ -714,6 +701,7 @@ DCMTrajectoryGeneratorHelper::DCMTrajectoryGeneratorHelper():
     m_lastStepDCMOffset(0),
     m_maxDoubleSupportDuration(-1),
     m_nominalDoubleSupportDuration(-1),
+    m_endSwitchTime(-1),
     m_pauseActive(false)
 {}
 
@@ -1015,9 +1003,11 @@ bool DCMTrajectoryGeneratorHelper::addNewStep(const double &singleSupportStartTi
     // evaluate the new Double Support phase
     std::shared_ptr<GeneralSupportTrajectory> newDoubleSupport = nullptr;
 
+    double doubleSupportDuration = doubleSupportFinalBoundaryCondition.time - doubleSupportInitBoundaryCondition.time;
+
     // check if pause features is active and if it is needed
     if (m_pauseActive &&
-            (doubleSupportFinalBoundaryCondition.time - doubleSupportInitBoundaryCondition.time > m_maxDoubleSupportDuration)){
+            (doubleSupportDuration > std::max(m_maxDoubleSupportDuration, m_endSwitchTime + m_nominalDoubleSupportDuration))){
         // in this case the DS phase is splitted in three DS phase
         // In the first one the DCM of the robot has to reach the center of the feet convex hull
         // In the second one the robot has to stop (stance phase)
@@ -1035,8 +1025,8 @@ bool DCMTrajectoryGeneratorHelper::addNewStep(const double &singleSupportStartTi
         // the constraints at the beginning and at the end of the double support stance phases are equal except for the times
         doubleSupportStanceFinalBoundaryCondition = doubleSupportStanceInitBoundaryCondition;
 
-        doubleSupportStanceInitBoundaryCondition.time = doubleSupportInitBoundaryCondition.time + m_nominalDoubleSupportDuration / 2;
-        doubleSupportStanceFinalBoundaryCondition.time = doubleSupportFinalBoundaryCondition.time - m_nominalDoubleSupportDuration / 2;
+        doubleSupportStanceInitBoundaryCondition.time = doubleSupportInitBoundaryCondition.time + m_endSwitchTime;
+        doubleSupportStanceFinalBoundaryCondition.time = doubleSupportFinalBoundaryCondition.time - m_nominalDoubleSupportDuration;
 
         // add the 3-th part of the Double Support phase
         newDoubleSupport = std::make_shared<DoubleSupportTrajectory>(doubleSupportStanceFinalBoundaryCondition,
@@ -1101,9 +1091,11 @@ bool DCMTrajectoryGeneratorHelper::addFirstDoubleSupportPhase(const DCMTrajector
     // evaluate the new Double Support phase
     std::shared_ptr<GeneralSupportTrajectory> newDoubleSupport = nullptr;
 
+    double doubleSupportDuration = doubleSupportFinalBoundaryCondition.time - doubleSupportInitBoundaryCondition.time;
+
     // check if pause features is active and if it is needed
     if (m_pauseActive &&
-            (doubleSupportFinalBoundaryCondition.time - doubleSupportInitBoundaryCondition.time > m_maxDoubleSupportDuration)){
+        (doubleSupportDuration > std::max(m_maxDoubleSupportDuration, m_endSwitchTime + m_nominalDoubleSupportDuration))) {
         // in this case the DS phase is splitted in three DS phase
         // In the first one the DCM of the robot has to reach the center of the feet convex hull
         // In the second one the robot has to stop (stance phase)
@@ -1128,8 +1120,8 @@ bool DCMTrajectoryGeneratorHelper::addFirstDoubleSupportPhase(const DCMTrajector
         // the constraints at the beginning and at the end of the double support stance phases are equal except for the times
         doubleSupportStanceFinalBoundaryCondition = doubleSupportStanceInitBoundaryCondition;
 
-        doubleSupportStanceInitBoundaryCondition.time = doubleSupportInitBoundaryCondition.time + m_nominalDoubleSupportDuration / 2;
-        doubleSupportStanceFinalBoundaryCondition.time = doubleSupportFinalBoundaryCondition.time - m_nominalDoubleSupportDuration / 2;
+        doubleSupportStanceInitBoundaryCondition.time = doubleSupportInitBoundaryCondition.time + m_endSwitchTime;
+        doubleSupportStanceFinalBoundaryCondition.time = doubleSupportFinalBoundaryCondition.time - m_nominalDoubleSupportDuration;
 
         // add the 3-th part of the Double Support phase
         newDoubleSupport = std::make_shared<DoubleSupportTrajectory>(doubleSupportStanceFinalBoundaryCondition,
@@ -1450,7 +1442,8 @@ void DCMTrajectoryGeneratorHelper::getWeightPercentage(std::vector<double> &weig
     weightInRight = m_weightInRight;
 }
 
-bool DCMTrajectoryGeneratorHelper::setPauseConditions(bool pauseActive, const double &maxDoubleSupportDuration, const double &nominalDoubleSupportDuration)
+bool DCMTrajectoryGeneratorHelper::setPauseConditions(bool pauseActive, const double &maxDoubleSupportDuration,
+                                                      const double &nominalDoubleSupportDuration, const double& endSwitchTime)
 {
     if (maxDoubleSupportDuration < 0){
         std::cerr << "[DCMTrajectoryGeneratorHelper::setPauseConditions] If the maxDoubleSupportDuration is negative, the robot won't pause in middle stance." << std::endl;
@@ -1468,13 +1461,25 @@ bool DCMTrajectoryGeneratorHelper::setPauseConditions(bool pauseActive, const do
             return false;
         }
 
-        if (nominalDoubleSupportDuration > maxDoubleSupportDuration){
-            std::cerr << "[DCMTrajectoryGeneratorHelper::setPauseConditions] The nominalDoubleSupportDuration cannot be greater than maxDoubleSupportDuration." << std::endl;
+        if (endSwitchTime < 0) {
+            std::cerr << "[DCMTrajectoryGeneratorHelper::setPauseConditions] The endSwitchTime is supposed to be positive." << std::endl;
             m_pauseActive = false;
             return false;
         }
+
+        if (nominalDoubleSupportDuration + endSwitchTime > maxDoubleSupportDuration) {
+            if (!m_timingWarningPrinted) {
+                std::cerr << "[DCMTrajectoryGeneratorHelper::setPauseConditions] Warning: the sum of nominalDoubleSupportDuration and endSwitchTime is greater than maxDoubleSupportDuration. ";
+                std::cerr << "The robot might not be able to pause in the middle of the stance phase." << std::endl;
+                m_timingWarningPrinted = true;
+            }
+        }
+        else {
+            m_timingWarningPrinted = false;
+        }
     }
     m_nominalDoubleSupportDuration = nominalDoubleSupportDuration;
+    m_endSwitchTime = endSwitchTime;
 
     return true;
 }

src/UnicycleGenerator.cpp

@@ -29,7 +29,7 @@ class UnicycleGenerator::UnicycleGeneratorImplementation {
     std::vector<size_t> mergePoints; //it stores the indeces from which is convenient to merge a new trajectory. The last element is the dimension of m_lFootPhases, i.e. merge after the end
     std::vector<bool> lFootContact, rFootContact, leftFixed;
 
-    double switchPercentage = 0.5, dT = 0.01, endSwitch = 0.0, initTime = 0.0;
+    double switchPercentage = 0.5, dT = 0.01, endSwitch = 1.0, initTime = 0.0;
     double nominalSwitchTime = 1.0;
     double maxStepTime = 10.0, nominalStepTime = 2.0;
     bool pauseActive = true;
@@ -118,14 +118,15 @@ class UnicycleGenerator::UnicycleGeneratorImplementation {
         lFootPhases->reserve(trajectoryDimension); // Notice that this dimension may not be the final dimension, due to rounding errors!!!
         rFootPhases->reserve(trajectoryDimension);
 
-        double stepTime, switchTime, pauseTime;
+        double stepTime, switchTime;
         size_t stepSamples, switchSamples, swingSamples;
 
         const StepList& leftList = lFootPrint.getSteps();
         const StepList& rightList = rFootPrint.getSteps();
         size_t orderedStepIndex = 2, leftIndex = 1, rightIndex = 1;
         const Step* nextStep;
         double previouStepTime = initTime;
+        double nominalSwingTime = nominalStepTime - nominalSwitchTime;
 
         while (orderedStepIndex < orderedSteps.size()){
             nextStep = orderedSteps[orderedStepIndex];
@@ -139,14 +140,11 @@ class UnicycleGenerator::UnicycleGeneratorImplementation {
             if ((orderedStepIndex == 2) && (lFootPrint.getSteps().front().impactTime != rFootPrint.getSteps().front().impactTime)) { //first half step
                 //Timings
                 switchTime = (switchPercentage/(1 - (switchPercentage/2.0)) * stepTime)/2.0; //half switch
-            } else { //general case
-                switchTime = switchPercentage * stepTime; //full switch
+            } else if(pauseActive && (stepTime > maxStepTime)) { //switch with pause
+                switchTime = stepTime - nominalSwingTime;
             }
-
-            bool pause = pauseActive && (stepTime > maxStepTime); //if true, it will pause in the middle
-            if (pause){
-                pauseTime = stepTime - nominalStepTime;
-                switchTime = nominalSwitchTime + pauseTime;
+            else { //general case
+                switchTime = switchPercentage * stepTime; //full switch
             }
 
             //Samples
@@ -173,13 +171,8 @@ class UnicycleGenerator::UnicycleGeneratorImplementation {
 
             if (orderedStepIndex != 2){ //add no merge point in the first half switch
                 size_t initialMergePoint, finalMergePoint;
-                if (pause){
-                    initialMergePoint = phaseShift.back() - static_cast<size_t>(std::round(nominalSwitchTime * (1 - mergePointRatioBegin)/(dT)));
-                    finalMergePoint = phaseShift.back() - static_cast<size_t>(std::round(nominalSwitchTime * (1 - mergePointRatioEnd)/(dT)));
-                } else {
-                    initialMergePoint = phaseShift.back() - static_cast<size_t>(std::round(switchTime * (1 - mergePointRatioBegin)/(dT)));
-                    finalMergePoint = phaseShift.back() - static_cast<size_t>(std::round(switchTime * (1 - mergePointRatioEnd)/(dT)));
-                }
+                initialMergePoint = phaseShift.back() - static_cast<size_t>(std::round(switchTime * (1 - mergePointRatioBegin)/(dT)));
+                finalMergePoint = phaseShift.back() - static_cast<size_t>(std::round(switchTime * (1 - mergePointRatioEnd)/(dT)));
 
                 for (size_t m = initialMergePoint; m <= finalMergePoint; ++m)
                     mergePoints.push_back(m);
@@ -434,7 +427,7 @@ bool UnicycleGenerator::generateFromFootPrints(std::shared_ptr<FootPrint> left,
     }
 
     if (m_pimpl->zmpGenerator) {
-        if (!(m_pimpl->zmpGenerator->computeNewTrajectories(initTime, dT, m_pimpl->switchPercentage, m_pimpl->maxStepTime,
+        if (!(m_pimpl->zmpGenerator->computeNewTrajectories(initTime, dT, m_pimpl->switchPercentage, m_pimpl->maxStepTime, m_pimpl->endSwitch,
                                                             m_pimpl->nominalStepTime, m_pimpl->pauseActive, m_pimpl->mergePoints,
                                                             *left, *right, m_pimpl->orderedSteps, *(m_pimpl->lFootPhases),
                                                             *(m_pimpl->rFootPhases), m_pimpl->phaseShift))) {
@@ -444,9 +437,9 @@ bool UnicycleGenerator::generateFromFootPrints(std::shared_ptr<FootPrint> left,
     }
 
     if (m_pimpl->dcmTrajectoryGenerator) {
-        if (!(m_pimpl->dcmTrajectoryGenerator->computeNewTrajectories(initTime, dT, m_pimpl->switchPercentage, m_pimpl->maxStepTime, m_pimpl->nominalStepTime,
-                                                                      m_pimpl->pauseActive, m_pimpl->orderedSteps, m_pimpl->phaseShift, *(m_pimpl->lFootPhases),
-                                                                      *left, *right))) {
+        if (!(m_pimpl->dcmTrajectoryGenerator->computeNewTrajectories(initTime, dT, m_pimpl->switchPercentage, m_pimpl->maxStepTime, m_pimpl->endSwitch,
+                                                                      m_pimpl->nominalStepTime, m_pimpl->pauseActive, m_pimpl->orderedSteps, m_pimpl->phaseShift,
+                                                                      *(m_pimpl->lFootPhases), *left, *right))) {
             std::cerr << "[UnicycleGenerator::generate] Failed while computing new DCM trajectories." << std::endl;
             return false;
         }
@@ -571,8 +564,8 @@ bool UnicycleGenerator::setTerminalHalfSwitchTime(double lastHalfSwitchTime)
 {
     std::lock_guard<std::mutex> guard(m_pimpl->mutex);
 
-    if (lastHalfSwitchTime < 0){
-        std::cerr << "[UnicycleGenerator::setTerminalHalfSwitchTime] The lastHalfSwitchTime cannot be negative." << std::endl;
+    if (lastHalfSwitchTime <= 0){
+        std::cerr << "[UnicycleGenerator::setTerminalHalfSwitchTime] The lastHalfSwitchTime has to be strictly positive." << std::endl;
         return false;
     }