Merge pull request #63 from robotology/fix_restart_issue

Reduction of DCM jumps when changing reference from an "almost" still configuration

.gitignore

@@ -38,3 +38,7 @@ build/*
 Thumbs.db
 .kdev4/*
 .vscode/
+
+# Visual Studio
+.vs/*
+CMakeSettings.json

CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.1)
 project(UnicyclePlanner
         LANGUAGES CXX
-        VERSION 0.7.0)
+        VERSION 0.8.0)
 
 # Defines the CMAKE_INSTALL_LIBDIR, CMAKE_INSTALL_BINDIR and many other useful macros
 # See https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html

include/DCMTrajectoryGenerator.h

@@ -57,6 +57,9 @@ class DCMTrajectoryGenerator {
 
     bool setDCMInitialState(const DCMInitialState &initialState);
 
+
+    const DCMInitialState & getDCMInitialState() const;
+
     /**
      * Set the time constant of the 3D-LIPM.
      * @param omega is the time constant of the 3D-LIPM.
@@ -77,6 +80,13 @@ class DCMTrajectoryGenerator {
      */
     bool setAlpha(const double &alpha);
 
+    /**
+     * Set the percentage of the last double support where the robot is completely still.
+     * @param alpha is the percentage of the last double support where the robot is completely still.
+     * @return true / false in case of success / failure.
+     */
+    bool setStillnessPercentage(const double& stillnessPercentage);
+
     /**
      * Set the last step DCM offset
      * @param lastStepDCMOffset Number from 0 to 1 used to indicate the position of the DCM w.r.t. the last ZMP position.

include/DCMTrajectoryGeneratorHelper.h

@@ -126,6 +126,7 @@ class DCMTrajectoryGeneratorHelper
     double m_dT; /**< Planner period. */
     double m_omega; /**< Time constant of the 3D-LIPM. */
     double m_alpha; /**< alpha is the parameter between zero and one for distributing the DS duration to SS phase. */
+    double m_stillnessPercentage; /**< Percentage of the last double support where the robot is completely still. */
     iDynTree::Vector2 m_leftZMPDelta; /**< Vector containing the desired left ZMP delta. */
     iDynTree::Vector2 m_rightZMPDelta; /**< Vector containing the desired left ZMP delta. */
     FirstDCMTrajectoryMode m_firstDCMTrajectoryMode{FirstDCMTrajectoryMode::ThirdOrderPoly}; /**< Mode of the first DS DCM trajectory */
@@ -246,6 +247,13 @@ class DCMTrajectoryGeneratorHelper
      */
     bool setAlpha(const double &alpha);
 
+    /**
+     * Set the percentage of the last double support where the robot is completely still.
+     * @param alpha is the percentage of the last double support where the robot is completely still.
+     * @return true / false in case of success / failure.
+     */
+    bool setStillnessPercentage(const double& stillnessPercentage);
+
     /**
      * Set the period of the Trajectory generator planner.
      * @param dT is the period (in seconds) of the Trajectory generator planner

src/DCMTrajectoryGenerator.cpp

@@ -128,6 +128,14 @@ bool DCMTrajectoryGenerator::setDCMInitialState(const DCMInitialState &initialSt
     return true;
 }
 
+const DCMInitialState & DCMTrajectoryGenerator::getDCMInitialState() const
+{
+
+    std::lock_guard<std::mutex> guard(m_pimpl->mutex);
+
+    return m_pimpl->initialState;
+}
+
 bool DCMTrajectoryGenerator::setOmega(const double &omega)
 {
     std::lock_guard<std::mutex> guard(m_pimpl->mutex);
@@ -149,6 +157,13 @@ bool DCMTrajectoryGenerator::setAlpha(const double &alpha)
     return m_pimpl->helper.setAlpha(alpha);
 }
 
+bool DCMTrajectoryGenerator::setStillnessPercentage(const double& stillnessPercentage)
+{
+    std::lock_guard<std::mutex> guard(m_pimpl->mutex);
+
+    return m_pimpl->helper.setStillnessPercentage(stillnessPercentage);
+}
+
 bool DCMTrajectoryGenerator::setLastStepDCMOffsetPercentage(const double &lastStepDCMOffset)
 {
     std::lock_guard<std::mutex> guard(m_pimpl->mutex);

src/DCMTrajectoryGeneratorHelper.cpp

@@ -710,7 +710,10 @@ DCMTrajectoryGeneratorHelper::DCMTrajectoryGeneratorHelper():
     m_dT(0.01),
     m_omega(9.81/0.5),
     m_alpha(0.5),
+    m_stillnessPercentage(0.1),
     m_lastStepDCMOffset(0),
+    m_maxDoubleSupportDuration(-1),
+    m_nominalDoubleSupportDuration(-1),
     m_pauseActive(false)
 {}
 
@@ -729,7 +732,7 @@ bool DCMTrajectoryGeneratorHelper::setOmega(const double &omega)
 bool DCMTrajectoryGeneratorHelper::setAlpha(const double &alpha)
 {
     if (alpha < 0 && alpha > 1){
-        std::cerr << "[DCMTrajectoryGeneratorHelper::setAlpha] The alpha sould be between zero and one."
+        std::cerr << "[DCMTrajectoryGeneratorHelper::setAlpha] The alpha should be between zero and one."
                   << std::endl;
         return false;
     }
@@ -738,6 +741,18 @@ bool DCMTrajectoryGeneratorHelper::setAlpha(const double &alpha)
     return true;
 }
 
+bool DCMTrajectoryGeneratorHelper::setStillnessPercentage(const double& stillnessPercentage)
+{
+    if (stillnessPercentage < 0 && stillnessPercentage > 1) {
+        std::cerr << "[DCMTrajectoryGeneratorHelper::setStillnessPercentage] The stillnessPercentage should be between zero and one."
+                  << std::endl;
+        return false;
+    }
+
+    m_stillnessPercentage = stillnessPercentage;
+    return true;
+}
+
 bool DCMTrajectoryGeneratorHelper::setdT(const double &dT)
 {
     if (dT < 0){
@@ -895,16 +910,29 @@ bool DCMTrajectoryGeneratorHelper::addLastStep(const double &singleSupportStartT
     newSingleSupport = std::make_shared<SingleSupportTrajectory>(singleSupportStartTime, singleSupportEndTime,
                                                                  m_omega, ZMP, singleSupportBoundaryCondition);
 
+
+    double lastStepPercentage = 1.0 - m_stillnessPercentage;
+
     // instantiate position and velocity boundary conditions vectors
     DCMTrajectoryPoint doubleSupportInitBoundaryCondition;
     DCMTrajectoryPoint doubleSupportFinalBoundaryCondition;
 
-    doubleSupportInitBoundaryCondition.time = singleSupportEndTime;
-    doubleSupportFinalBoundaryCondition.time = doubleSupportEndTime;
+    // instantiate position and velocity boundary conditions vectors
+    DCMTrajectoryPoint finalDoubleSupportInitBoundaryCondition;
+    DCMTrajectoryPoint finalDoubleSupportFinalBoundaryCondition;
+
 
+    doubleSupportInitBoundaryCondition.time = singleSupportEndTime;
+    doubleSupportFinalBoundaryCondition.time = singleSupportEndTime + (doubleSupportEndTime - singleSupportEndTime) * lastStepPercentage;
     doubleSupportInitBoundaryCondition.DCMPosition = singleSupportBoundaryCondition.DCMPosition;
     doubleSupportFinalBoundaryCondition.DCMPosition = doubleSupportEndPosition;
 
+    finalDoubleSupportInitBoundaryCondition.time = doubleSupportFinalBoundaryCondition.time;
+    finalDoubleSupportFinalBoundaryCondition.time = doubleSupportEndTime;
+    finalDoubleSupportInitBoundaryCondition.DCMPosition = doubleSupportEndPosition;
+    finalDoubleSupportFinalBoundaryCondition.DCMPosition = doubleSupportEndPosition;
+
+
     if(!newSingleSupport->getDCMVelocity(doubleSupportInitBoundaryCondition.time,
                                          doubleSupportInitBoundaryCondition.DCMVelocity, true, m_dT/2)){
         std::cerr << "[DCMTrajectoryGeneratorHelper::addLastStep] Error when the velocity of the DCM in the next SS phase is evaluated." <<std::endl;
@@ -913,9 +941,20 @@ bool DCMTrajectoryGeneratorHelper::addLastStep(const double &singleSupportStartT
     // only for the last step the final velocity of the DCM is zero
     doubleSupportFinalBoundaryCondition.DCMVelocity.zero();
 
+    finalDoubleSupportFinalBoundaryCondition.DCMVelocity.zero();
+    finalDoubleSupportInitBoundaryCondition.DCMVelocity.zero();
+
+
+    std::shared_ptr<DoubleSupportTrajectory> newFinalDoubleSupport = std::make_shared<DoubleSupportTrajectory>(finalDoubleSupportInitBoundaryCondition,
+													       finalDoubleSupportFinalBoundaryCondition,
+													       m_omega);
+
     std::shared_ptr<DoubleSupportTrajectory> newDoubleSupport = std::make_shared<DoubleSupportTrajectory>(doubleSupportInitBoundaryCondition,
                                                                                                           doubleSupportFinalBoundaryCondition,
                                                                                                           m_omega);
+
+    // add the final Double Support phase
+    m_trajectory.push_back(newFinalDoubleSupport);
     // add the new Double Support phase
     m_trajectory.push_back(newDoubleSupport);