Implement a logic to force the rest condition when the dcm velocity i…

…s slow
robotology · Jan 29, 2024 · a129888 · a129888
1 parent 494a182
commit a129888
Show file tree

Hide file tree

Showing 4 changed files with 52 additions and 2 deletions.
diff --git a/include/DCMTrajectoryGenerator.h b/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.

diff --git a/src/DCMTrajectoryGenerator.cpp b/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);

diff --git a/src/DCMTrajectoryGeneratorHelper.cpp b/src/DCMTrajectoryGeneratorHelper.cpp
@@ -895,16 +895,29 @@ bool DCMTrajectoryGeneratorHelper::addLastStep(const double &singleSupportStartT
     newSingleSupport = std::make_shared<SingleSupportTrajectory>(singleSupportStartTime, singleSupportEndTime,
                                                                  m_omega, ZMP, singleSupportBoundaryCondition);
 
+
+    const double lastStepPercentage = 0.90;
+
     // 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 +926,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);
 

diff --git a/src/UnicycleGenerator.cpp b/src/UnicycleGenerator.cpp
@@ -14,6 +14,8 @@
 #include <cstddef>
 #include <mutex>
 
+#include <iDynTree/Core/EigenHelpers.h>
+
 typedef StepList::const_iterator StepsIndex;
 
 class UnicycleGenerator::UnicycleGeneratorImplementation {
@@ -486,6 +488,19 @@ bool UnicycleGenerator::reGenerate(double initTime, double dT, double endTime, b
         m_pimpl->leftFootPrint->getLastStep(previousL);
         m_pimpl->rightFootPrint->getLastStep(previousR);
 
+
+	if (m_pimpl->dcmTrajectoryGenerator != nullptr
+	    && iDynTree::toEigen(m_pimpl->dcmTrajectoryGenerator->getDCMInitialState().initialVelocity).norm() < 1e-5)
+	{
+	    double max = std::max(previousL.impactTime, previousR.impactTime);
+	    previousL.impactTime = previousR.impactTime = max;
+
+	    m_pimpl->leftFootPrint->clearSteps();
+	    m_pimpl->rightFootPrint->clearSteps();
+	    m_pimpl->leftFootPrint->addStep(previousL);
+	    m_pimpl->rightFootPrint->addStep(previousR);	    
+	}
+
         std::shared_ptr<FootPrint> toBeCorrected = correctLeft ? m_pimpl->leftFootPrint : m_pimpl->rightFootPrint;
 
         correctedStep = correctLeft ? previousL : previousR;