Modify FSM to solve instability issues at the beginning

- Define NbStepsSSDS_ to be always the least nb of steps

src/PreviewControl/SupportFSM.cpp

@@ -55,9 +55,10 @@ void SupportFSM::set_support_state(const double &Time, const int &pi,
     ReferenceGiven = true;
 
   // Update time limit for double support phase
-  if(ReferenceGiven == true && Support.Phase == 0 && (Support.TimeLimit-Time-eps)>DSSSPeriod_)
+  if(ReferenceGiven && Support.Phase == 0 && (Support.TimeLimit-Time-eps) > DSSSPeriod_)
     {
       Support.TimeLimit = Time+DSSSPeriod_;
+      Support.NbStepsLeft = NbStepsSSDS_;
     }
 
   //FSM
@@ -65,14 +66,14 @@ void SupportFSM::set_support_state(const double &Time, const int &pi,
     {
 
       //SS->DS
-      if(Support.Phase == 1  && ReferenceGiven == false && Support.NbStepsLeft==0)
+      if(Support.Phase == 1  && !ReferenceGiven && Support.NbStepsLeft == 0)
 	{
 	  Support.Phase = 0;
 	  Support.TimeLimit = Time+pi*T_ + DSPeriod_;
 	  Support.StateChanged = true;
 	}
       //DS->SS
-      else if(Support.Phase == 0 && ReferenceGiven == true)
+      else if(Support.Phase == 0 && ReferenceGiven || Support.Phase == 0 && Support.NbStepsLeft > 0)
 	{
 	  Support.Phase = 1;
 	  Support.TimeLimit = Time+pi*T_ + StepPeriod_;
@@ -80,15 +81,17 @@ void SupportFSM::set_support_state(const double &Time, const int &pi,
 	  Support.StateChanged = true;
 	}
       //SS->SS
-      else if(((Support.Phase == 1) && (Support.NbStepsLeft>0)) ||
-	      ((Support.NbStepsLeft == 0) && (ReferenceGiven == true)))
+      else if(Support.Phase == 1 && Support.NbStepsLeft > 0 ||
+	      Support.NbStepsLeft == 0 && ReferenceGiven)
 	{
 	  Support.Foot = -1*Support.Foot;
 	  Support.StateChanged = true;
 	  Support.TimeLimit = Time+pi*T_ + StepPeriod_;
 	  Support.StepNumber++;
-	  if (ReferenceGiven == false)
+	  if (!ReferenceGiven)
 	    Support.NbStepsLeft = Support.NbStepsLeft-1;
+	  if (ReferenceGiven)
+	    Support.NbStepsLeft = NbStepsSSDS_;
 	}
     }
 

src/PreviewControl/SupportFSM.h

@@ -83,7 +83,7 @@ namespace PatternGeneratorJRL
     //
   private: 
 
-    /// \brief Number of steps done before DS
+    /// \brief Number of steps to be done before DS
     unsigned NbStepsSSDS_;
 
     /// \brief Length of a double support phase

src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp

@@ -79,7 +79,7 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
   SupportFSM_->StepPeriod(0.8);
   SupportFSM_->DSPeriod(1e9);
   SupportFSM_->DSSSPeriod(0.8);
-  SupportFSM_->NbStepsSSDS(200);
+  SupportFSM_->NbStepsSSDS(2);
   SupportFSM_->SamplingPeriod(QP_T_);
 
   /* Orientations preview algorithm*/