diff --git a/.github/workflows/conda-forge-ci.yml b/.github/workflows/conda-forge-ci.yml index 742c098..1cd1b9f 100644 --- a/.github/workflows/conda-forge-ci.yml +++ b/.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,18 +23,17 @@ 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} @@ -42,7 +41,7 @@ jobs: 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: | diff --git a/include/DCMTrajectoryGenerator.h b/include/DCMTrajectoryGenerator.h index 9e597d0..bc8a65d 100644 --- a/include/DCMTrajectoryGenerator.h +++ b/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 &orderedSteps, const std::vector &phaseShift, const std::vector &lFootPhases, const FootPrint &left, const FootPrint &right); diff --git a/include/DCMTrajectoryGeneratorHelper.h b/include/DCMTrajectoryGeneratorHelper.h index eadc014..c254fbc 100644 --- a/include/DCMTrajectoryGeneratorHelper.h +++ b/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> m_trajectory; /**< Vector containing pointer of every trajectory phase. */ @@ -152,6 +153,8 @@ class DCMTrajectoryGeneratorHelper std::vector m_ZMPPosition; /**< Vector containing the position of the ZMP. */ std::vector 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 diff --git a/include/UnicycleGenerator.h b/include/UnicycleGenerator.h index 5cee98d..24312a8 100644 --- a/include/UnicycleGenerator.h +++ b/include/UnicycleGenerator.h @@ -51,7 +51,7 @@ 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); @@ -59,6 +59,7 @@ class UnicycleGenerator { void disablePauseConditions(); + [[deprecated("Use unicyclePlanner()->setNavigationPath(path) instead")]] bool setNavigationPath(const std::vector &path); /** diff --git a/include/ZMPTrajectoryGenerator.h b/include/ZMPTrajectoryGenerator.h index e79402c..d8e39cd 100644 --- a/include/ZMPTrajectoryGenerator.h +++ b/include/ZMPTrajectoryGenerator.h @@ -25,7 +25,7 @@ class ZMPTrajectoryGenerator { class ZMPTrajectoryGeneratorImplementation; std::unique_ptr 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 &mergePoints, const FootPrint &left, const FootPrint &right, const std::vector& orderedSteps, diff --git a/src/DCMTrajectoryGenerator.cpp b/src/DCMTrajectoryGenerator.cpp index 2c9cd4f..70eb8ac 100644 --- a/src/DCMTrajectoryGenerator.cpp +++ b/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 &orderedSteps, const std::vector &phaseShift, const std::vector &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; } diff --git a/src/DCMTrajectoryGeneratorHelper.cpp b/src/DCMTrajectoryGeneratorHelper.cpp index be91fc6..192bf52 100644 --- a/src/DCMTrajectoryGeneratorHelper.cpp +++ b/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 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(doubleSupportStanceFinalBoundaryCondition, @@ -1101,9 +1091,11 @@ bool DCMTrajectoryGeneratorHelper::addFirstDoubleSupportPhase(const DCMTrajector // evaluate the new Double Support phase std::shared_ptr 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(doubleSupportStanceFinalBoundaryCondition, @@ -1450,7 +1442,8 @@ void DCMTrajectoryGeneratorHelper::getWeightPercentage(std::vector &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; } diff --git a/src/UnicycleGenerator.cpp b/src/UnicycleGenerator.cpp index d7d9fc8..db652c9 100644 --- a/src/UnicycleGenerator.cpp +++ b/src/UnicycleGenerator.cpp @@ -29,7 +29,7 @@ class UnicycleGenerator::UnicycleGeneratorImplementation { std::vector 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 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,7 +118,7 @@ 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(); @@ -126,6 +126,7 @@ class UnicycleGenerator::UnicycleGeneratorImplementation { 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(std::round(nominalSwitchTime * (1 - mergePointRatioBegin)/(dT))); - finalMergePoint = phaseShift.back() - static_cast(std::round(nominalSwitchTime * (1 - mergePointRatioEnd)/(dT))); - } else { - initialMergePoint = phaseShift.back() - static_cast(std::round(switchTime * (1 - mergePointRatioBegin)/(dT))); - finalMergePoint = phaseShift.back() - static_cast(std::round(switchTime * (1 - mergePointRatioEnd)/(dT))); - } + initialMergePoint = phaseShift.back() - static_cast(std::round(switchTime * (1 - mergePointRatioBegin)/(dT))); + finalMergePoint = phaseShift.back() - static_cast(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 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 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 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; } diff --git a/src/ZMPTrajectoryGenerator.cpp b/src/ZMPTrajectoryGenerator.cpp index 4c6057b..0d3d2ce 100644 --- a/src/ZMPTrajectoryGenerator.cpp +++ b/src/ZMPTrajectoryGenerator.cpp @@ -21,7 +21,7 @@ class ZMPTrajectoryGenerator::ZMPTrajectoryGeneratorImplementation { std::vector initStates; - double nominalSwitchTime, maxSwitchTime, maxSwingTime, nominalSwingTime; + double nominalSwitchTime, maxSwitchTime, maxSwingTime, nominalSwingTime, endSwitchTime; iDynTree::Vector2 leftStanceZMP, leftSwitchZMP, rightStanceZMP, rightSwitchZMP; Step previousLeft, previousRight; @@ -34,6 +34,7 @@ class ZMPTrajectoryGenerator::ZMPTrajectoryGeneratorImplementation { std::vector leftZMPAcceleration, rightZMPAcceleration, worldZMPAcceleration; bool initialWeightSpecified = false, previousStepsSpecified = false; + bool timingWarningPrinted = false; std::mutex mutex; @@ -98,8 +99,7 @@ class ZMPTrajectoryGenerator::ZMPTrajectoryGeneratorImplementation { } else if ((stepPhase[instant] == StepPhase::SwitchIn)||(stepPhase[instant] == StepPhase::SwitchOut)){ switchLength = (endOfPhase-instant) * dT; - bool pause = pauseActive && (switchLength > maxSwitchTime); //if true, it will pause in the middle - + bool pause = pauseActive && (switchLength > std::max(maxSwitchTime, endSwitchTime + nominalSwitchTime)); //if true, it will pause in the middle if (phase == 1){ //first half switch m_buffer(0) = initialState.initialPosition; m_spline.setInitialConditions(initialState.initialVelocity, initialState.initialAcceleration); @@ -112,14 +112,14 @@ class ZMPTrajectoryGenerator::ZMPTrajectoryGeneratorImplementation { if (pause){ m_buffer(1) = 0.5; //pause in the middle m_spline.setFinalConditions(0.0, 0.0); - m_timesBuffer(1) = nominalSwitchTime/2; + m_timesBuffer(1) = endSwitchTime; if (!m_spline.setData(m_timesBuffer, m_buffer)){ std::cerr << "[ZMPTrajectoryGenerator::computeNewTrajectories] Failed to initialize the spline for the weight portion during pause." << std::endl; return false; } initialSwitchInstant = instant; - while (instant < (initialSwitchInstant + std::round(nominalSwitchTime/(2.0 * dT)))){ + while (instant < (initialSwitchInstant + std::round(endSwitchTime / dT))){ switchInstant = (instant - initialSwitchInstant)*dT; output[instant] = m_spline.evaluatePoint(switchInstant, outputVelocity[instant], outputAcceleration[instant]); @@ -135,7 +135,7 @@ class ZMPTrajectoryGenerator::ZMPTrajectoryGeneratorImplementation { ++instant; } - while (instant < (endOfPhase - std::round(nominalSwitchTime/(2.0 * dT)))){ + while (instant < (endOfPhase - std::round(nominalSwitchTime/ dT))){ output[instant] = 0.5; outputVelocity[instant] = 0.0; outputAcceleration[instant] = 0.0; @@ -300,7 +300,7 @@ class ZMPTrajectoryGenerator::ZMPTrajectoryGeneratorImplementation { } } else { switchLength = (endOfPhase-instant) * dT; - bool pause = pauseActive && (switchLength > maxSwitchTime); //if true, it will pause in the middle + bool pause = pauseActive && (switchLength > std::max(maxSwitchTime,endSwitchTime + nominalSwitchTime)); //if true, it will pause in the middle m_xBuffer(0) = switchZmpInitPosition(0); m_yBuffer(0) = switchZmpInitPosition(1); @@ -311,7 +311,7 @@ class ZMPTrajectoryGenerator::ZMPTrajectoryGeneratorImplementation { m_xBuffer(1) = stanceZmpPosition(0); //bring the ZMP back to the stance position m_yBuffer(1) = stanceZmpPosition(1); if (pause){ - m_timesBuffer(1) = nominalSwitchTime/2; + m_timesBuffer(1) = endSwitchTime; } else if (phase == (phaseShift.size() - 1)){ m_timesBuffer(1) = (endOfPhase - instant)*dT; } else { @@ -506,7 +506,7 @@ class ZMPTrajectoryGenerator::ZMPTrajectoryGeneratorImplementation { -bool ZMPTrajectoryGenerator::computeNewTrajectories(double initTime, double dT, double switchPercentage, double maxStepTime, +bool ZMPTrajectoryGenerator::computeNewTrajectories(double initTime, double dT, double switchPercentage, double maxStepTime, double endSwitchTime, double nominalStepTime, bool pauseActive, const std::vector &mergePoints, const FootPrint &left, const FootPrint &right, const std::vector &orderedSteps, const std::vector &lFootPhases, const std::vector &rFootPhases, const std::vector &phaseShift) @@ -517,6 +517,18 @@ bool ZMPTrajectoryGenerator::computeNewTrajectories(double initTime, double dT, m_pimpl->maxSwitchTime = switchPercentage * maxStepTime; m_pimpl->maxSwingTime = maxStepTime - m_pimpl->maxSwitchTime; m_pimpl->nominalSwingTime = nominalStepTime - m_pimpl->nominalSwitchTime; + m_pimpl->endSwitchTime = endSwitchTime; + + if (m_pimpl->nominalSwitchTime + endSwitchTime > m_pimpl->maxSwitchTime) { + if (!m_pimpl->timingWarningPrinted) { + std::cerr << "[ZMPTrajectoryGenerator::computeNewTrajectories] Warning: the sum of nominalSwitchTime and endSwitchTime is greater than maxSwitchTime. "; + std::cerr << "The robot might not be able to pause in the middle of the stance phase." << std::endl; + m_pimpl->timingWarningPrinted = true; + } + } + else { + m_pimpl->timingWarningPrinted = false; + } if (!(m_pimpl->initialWeightSpecified)) { m_pimpl->initialState.initialPosition = 0.5;