debug SQP feedback.

get continuous trajectory as output, still diverge with the dynamic filter

src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp

@@ -108,9 +108,21 @@ void
       || LocalInterpolationStartTime +InterpolationTime >= StartLanding)
     {
       // Do not modify x, y and theta while liftoff.
-      curr_NSFAP.x = prev_NSFAP.x;
-      curr_NSFAP.y = prev_NSFAP.y;
-      curr_NSFAP.theta = prev_NSFAP.theta;
+      curr_NSFAP.x        = prev_NSFAP.x;
+      curr_NSFAP.y        = prev_NSFAP.y;
+      curr_NSFAP.theta    = prev_NSFAP.theta;
+      curr_NSFAP.dx       = 0.0;
+      curr_NSFAP.dy       = 0.0;
+      curr_NSFAP.dtheta   = 0.0;
+      curr_NSFAP.ddx      = 0.0;
+      curr_NSFAP.ddy      = 0.0;
+      curr_NSFAP.ddtheta  = 0.0;
+      curr_NSFAP.ddx      = 0.0;
+      curr_NSFAP.ddy      = 0.0;
+      curr_NSFAP.ddtheta  = 0.0;
+      curr_NSFAP.dddx     = 0.0;
+      curr_NSFAP.dddy     = 0.0;
+      curr_NSFAP.dddtheta = 0.0;
       // And all the derivatives are null
     }
   else if (LocalInterpolationStartTime < EndOfLiftOff)
@@ -411,7 +423,7 @@ void OnLineFootTrajectoryGeneration::interpolate_feet_positions(
 
   if(CurrentIndex < 5)
   {
-    cout << Time+1.5*QP_T_ << " <? " << CurrentSupport.TimeLimit << endl ;
+    cout << CurrentSupport.Phase << " && " <<  Time+1.5*QP_T_ << " < " << CurrentSupport.TimeLimit << " : " << (CurrentSupport.Phase == SS && Time+1.5*QP_T_ < CurrentSupport.TimeLimit) <<endl ;
   }
   if(CurrentSupport.Phase == SS && Time+1.5*QP_T_ < CurrentSupport.TimeLimit)
   {

src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp

@@ -163,7 +163,8 @@ int CoMAndFootOnlyStrategy::EvaluateStartingState(MAL_VECTOR(&,double) BodyAngle
   // The  altitude of the zmp depend on the altitude of the support foot.
   aStartingZMPPosition(2) = 0.5 * (InitLeftFootPosition.z + InitRightFootPosition.z) ;
 
-  //  cerr << "YOU SHOULD INITIALIZE PROPERLY aStartingZMPosition in   CoMAndFootOnlyStrategy::EvaluateStartingState" <<endl;
+  //  cerr << "YOU SHOULD INITIALIZE PROPERLY aStartingZMPosition in \
+  //  CoMAndFootOnlyStrategy::EvaluateStartingState" <<endl;
 
 //  cout << "com = " << aStartingCOMState  << endl ;
 //  cout << "zmp = " << aStartingZMPPosition  << endl ;

src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp

@@ -424,7 +424,7 @@ void ZMPVelocityReferencedSQP::OnLine(double time,
     }
     VelRef_=NewVelRef_;
     NMPCgenerator_->updateInitialCondition(
-        time+(SQP_T_-TimeBuffer_-m_SamplingPeriod),
+        time,
         initLeftFoot_ ,
         initRightFoot_,
         initCOM_,
@@ -437,12 +437,12 @@ void ZMPVelocityReferencedSQP::OnLine(double time,
     // INITIALIZE INTERPOLATION:
     // ------------------------
     CurrentIndex_ = FinalCOMTraj_deq.size();
-    for (unsigned int i = 0  ; i <= CurrentIndex_ ; ++i )
+    for (unsigned int i = 0  ; i < NbSampleControl_ + CurrentIndex_ ; ++i )
     {
-        ZMPTraj_deq_ctrl_[i] = FinalZMPTraj_deq[i] ;
-        COMTraj_deq_ctrl_[i] = FinalCOMTraj_deq[i] ;
-        LeftFootTraj_deq_ctrl_ [i]= FinalLeftFootTraj_deq  [i] ;
-        RightFootTraj_deq_ctrl_[i]= FinalRightFootTraj_deq [i] ;
+        ZMPTraj_deq_ctrl_[i] = initZMP_ ;
+        COMTraj_deq_ctrl_[i] = initCOM_ ;
+        LeftFootTraj_deq_ctrl_[i] = initLeftFoot_ ;
+        RightFootTraj_deq_ctrl_[i] = initRightFoot_ ;
     }
     FinalZMPTraj_deq.resize( NbSampleControl_ + CurrentIndex_);
     FinalCOMTraj_deq.resize( NbSampleControl_ + CurrentIndex_);
@@ -457,10 +457,10 @@ void ZMPVelocityReferencedSQP::OnLine(double time,
     // Take only the data that are actually used by the robot
     for(unsigned i=0 ; i < FinalZMPTraj_deq.size() ; ++i)
     {
-      FinalZMPTraj_deq      [i] = ZMPTraj_deq_ctrl_      [i/*+CurrentIndex_*/] ;
-      FinalCOMTraj_deq      [i] = COMTraj_deq_ctrl_      [i/*+CurrentIndex_*/] ;
-      FinalLeftFootTraj_deq [i] = LeftFootTraj_deq_ctrl_ [i/*+CurrentIndex_*/] ;
-      FinalRightFootTraj_deq[i] = RightFootTraj_deq_ctrl_[i/*+CurrentIndex_*/] ;
+      FinalZMPTraj_deq      [i] = ZMPTraj_deq_ctrl_      [i] ;
+      FinalCOMTraj_deq      [i] = COMTraj_deq_ctrl_      [i] ;
+      FinalLeftFootTraj_deq [i] = LeftFootTraj_deq_ctrl_ [i] ;
+      FinalRightFootTraj_deq[i] = RightFootTraj_deq_ctrl_[i] ;
     }
 
     bool filterOn_ = true ;
@@ -519,14 +519,14 @@ void ZMPVelocityReferencedSQP::FullTrajectoryInterpolation(double time)
 {
   if(LeftFootTraj_deq_ctrl_.size() < CurrentIndex_ + previewSize_ * NbSampleControl_)
   {
-    COMTraj_deq_ctrl_      .resize(CurrentIndex_ + previewSize_ * NbSampleControl_
-                                 + CurrentIndexUpperBound_);
-    ZMPTraj_deq_ctrl_      .resize(CurrentIndex_ + previewSize_ * NbSampleControl_
-                                 + CurrentIndexUpperBound_);
-    LeftFootTraj_deq_ctrl_ .resize(CurrentIndex_ + previewSize_ * NbSampleControl_
-                                 + CurrentIndexUpperBound_);
+    COMTraj_deq_ctrl_.resize(CurrentIndex_ + previewSize_ * NbSampleControl_
+                             + CurrentIndexUpperBound_,initCOM_);
+    ZMPTraj_deq_ctrl_.resize(CurrentIndex_ + previewSize_ * NbSampleControl_
+                             + CurrentIndexUpperBound_,initZMP_);
+    LeftFootTraj_deq_ctrl_.resize(CurrentIndex_ + previewSize_ * NbSampleControl_
+                             + CurrentIndexUpperBound_,initLeftFoot_);
     RightFootTraj_deq_ctrl_.resize(CurrentIndex_ + previewSize_ * NbSampleControl_
-                                 + CurrentIndexUpperBound_);
+                             + CurrentIndexUpperBound_,initRightFoot_);
   }
 
   std::vector<double> JerkX ;
@@ -543,14 +543,14 @@ void ZMPVelocityReferencedSQP::FullTrajectoryInterpolation(double time)
 
   support_state_t currentSupport = SupportStates_deq[0] ;
   currentSupport.StepNumber=0;
-  OFTG_->interpolate_feet_positions(time+m_SamplingPeriod*CurrentIndex_, CurrentIndex_, currentSupport,
+  OFTG_->interpolate_feet_positions(time/*m_SamplingPeriod*CurrentIndex_*/, CurrentIndex_, currentSupport,
                                   FootStepX, FootStepY, FootStepYaw,
                                   LeftFootTraj_deq_ctrl_, RightFootTraj_deq_ctrl_);
 
   for ( int i = 1 ; i<previewSize_ ; i++ )
   {
     CoMZMPInterpolation(JerkX,JerkY,&LIPM_,NbSampleControl_,i,CurrentIndex_,SupportStates_deq);
-    OFTG_->interpolate_feet_positions(time+m_SamplingPeriod*CurrentIndex_+i*SQP_T_, CurrentIndex_ + i * NbSampleControl_,
+    OFTG_->interpolate_feet_positions(time/*+m_SamplingPeriod*CurrentIndex_*/+i*SQP_T_, CurrentIndex_ + i * NbSampleControl_,
                                       SupportStates_deq[i],
                                       FootStepX, FootStepY, FootStepYaw,
                                       LeftFootTraj_deq_ctrl_, RightFootTraj_deq_ctrl_);
@@ -576,12 +576,12 @@ void ZMPVelocityReferencedSQP::FullTrajectoryInterpolation(double time)
 
   COMTraj_deq_ctrl_      .push_back(COMTraj_deq_ctrl_      .back());
   ZMPTraj_deq_ctrl_      .push_back(ZMPTraj_deq_ctrl_      .back());
-//  LeftFootTraj_deq_ctrl_ .push_back(LeftFootTraj_deq_ctrl_ .back());
-//  RightFootTraj_deq_ctrl_.push_back(RightFootTraj_deq_ctrl_.back());
+  LeftFootTraj_deq_ctrl_ .push_back(LeftFootTraj_deq_ctrl_ .back());
+  RightFootTraj_deq_ctrl_.push_back(RightFootTraj_deq_ctrl_.back());
   COMTraj_deq_ctrl_      .pop_front();
   ZMPTraj_deq_ctrl_      .pop_front();
-//  LeftFootTraj_deq_ctrl_ .pop_front();
-//  RightFootTraj_deq_ctrl_.pop_front();
+  LeftFootTraj_deq_ctrl_ .pop_front();
+  RightFootTraj_deq_ctrl_.pop_front();
 
   unsigned int IndexMax = (int)round(previewDuration_/InterpolationPeriod_ );
   ZMPTraj_deq_.resize(IndexMax);