Modification of the buffers for the Kajita PC algorithm, orientation problem about the CoM

src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp

@@ -442,66 +442,77 @@ ZMPVelocityReferencedQP::OnLine(double time,
 
       // INTERPOLATE THE NEXT COMPUTED COM STATE:
       // ----------------------------------------
-      unsigned currentIndex = COMTraj_deq.size();
+      unsigned currentIndex = FinalCOMTraj_deq.size();
       unsigned numberOfSample = (unsigned)(QP_T_/m_SamplingPeriod);
 
-      deque<ZMPPosition> ZMPTraj_deq ( QP_N_ * numberOfSample + currentIndex );
-      deque<COMState> COMTraj_deq ( QP_N_ * numberOfSample + currentIndex );
-      deque<FootAbsolutePosition> LeftFootTraj_deq ( FinalLeftFootTraj_deq.size() );
-      deque<FootAbsolutePosition> RightFootTraj_deq ( FinalRightFootTraj_deq.size() );
+      deque<ZMPPosition> ZMPTraj_deq (FinalZMPTraj_deq);
+      deque<COMState> COMTraj_deq (FinalCOMTraj_deq);
+      deque<FootAbsolutePosition> LeftFootTraj_deq (FinalLeftFootTraj_deq);
+      deque<FootAbsolutePosition> RightFootTraj_deq (FinalRightFootTraj_deq);
 
-      for (unsigned int i = 0 ; i < FinalZMPTraj_deq.size() ; i++ )
-      {
-        ZMPTraj_deq[i] = FinalZMPTraj_deq[i] ;
-        COMTraj_deq[i] = FinalCOMTraj_deq[i] ;
-        LeftFootTraj_deq[i] = FinalLeftFootTraj_deq[i] ;
-        RightFootTraj_deq[i] = FinalRightFootTraj_deq[i] ;
-      }
-
-      FinalCOMTraj_deq.resize( numberOfSample + currentIndex );
-      FinalZMPTraj_deq.resize( numberOfSample + currentIndex );
-      FinalLeftFootTraj_deq.resize( numberOfSample + currentIndex );
-      FinalRightFootTraj_deq.resize( numberOfSample + currentIndex );
+      ZMPTraj_deq.resize( QP_N_ * numberOfSample + currentIndex );
+      COMTraj_deq.resize( QP_N_ * numberOfSample + currentIndex );
+      LeftFootTraj_deq.resize( QP_N_ * numberOfSample + currentIndex );
+      RightFootTraj_deq.resize( QP_N_ * numberOfSample + currentIndex );
 
       for ( int i = 0 ; i < QP_N_ ; i++ )
       {
         if(Solution_.SupportStates_deq.size() &&  Solution_.SupportStates_deq[0].NbStepsLeft == 0)
         {
-           double jx = (LeftFootTraj_deq[0].x + RightFootTraj_deq[0].x)/2 - COMTraj_deq[0].x[0];
-           double jy = (LeftFootTraj_deq[0].y + RightFootTraj_deq[0].y)/2 - COMTraj_deq[0].y[0];
-           if(fabs(jx) < 1e-3 && fabs(jy) < 1e-3) { Running_ = false; }
-           const double tf = 0.75;
-           jx = 6/(tf*tf*tf)*(jx - tf*COMTraj_deq[0].x[1] - (tf*tf/2)*COMTraj_deq[0].x[2]);
-           jy = 6/(tf*tf*tf)*(jy - tf*COMTraj_deq[0].y[1] - (tf*tf/2)*COMTraj_deq[0].y[2]);
-           CoM_.Interpolation( COMTraj_deq, ZMPTraj_deq, currentIndex + numberOfSample * i,
-               jx, jy);
-           CoM_.OneIteration( jx, jy );
+          double jx = (FinalLeftFootTraj_deq[0].x + FinalRightFootTraj_deq[0].x)/2 - FinalCOMTraj_deq[0].x[0];
+          double jy = (FinalLeftFootTraj_deq[0].y + FinalRightFootTraj_deq[0].y)/2 - FinalCOMTraj_deq[0].y[0];
+          if(fabs(jx) < 1e-3 && fabs(jy) < 1e-3) { Running_ = false; }
+          const double tf = 0.75;
+          jx = 6/(tf*tf*tf)*(jx - tf*FinalCOMTraj_deq[0].x[1] - (tf*tf/2)*FinalCOMTraj_deq[0].x[2]);
+          jy = 6/(tf*tf*tf)*(jy - tf*FinalCOMTraj_deq[0].y[1] - (tf*tf/2)*FinalCOMTraj_deq[0].y[2]);
+          CoM_.Interpolation( COMTraj_deq, ZMPTraj_deq, currentIndex + i * numberOfSample,
+                              jx, jy);
+          if(i == 0)
+          {
+            CoM_.OneIteration( jx, jy );
+          }
         }
         else
         {
-           Running_ = true;
-           CoM_.Interpolation( COMTraj_deq, ZMPTraj_deq, currentIndex + numberOfSample * i,
+          Running_ = true;
+          CoM_.Interpolation( COMTraj_deq, ZMPTraj_deq, currentIndex + i * numberOfSample,
                Solution_.Solution_vec[i], Solution_.Solution_vec[QP_N_+i] );
-           CoM_.OneIteration( Solution_.Solution_vec[i],Solution_.Solution_vec[QP_N_+i] );
+          if(i == 0)
+          {
+            CoM_.OneIteration( Solution_.Solution_vec[0],Solution_.Solution_vec[QP_N_] );
+          }
         }
 
         // INTERPOLATE TRUNK ORIENTATION:
         // ------------------------------
-        OrientPrw_->interpolate_trunk_orientation( time, currentIndex + numberOfSample * i,
+        OrientPrw_->interpolate_trunk_orientation( time + i * QP_T_, currentIndex + i * numberOfSample,
             m_SamplingPeriod, Solution_.SupportStates_deq,
             COMTraj_deq );
 
         // INTERPOLATE THE COMPUTED FOOT POSITIONS:
         // ----------------------------------------
-        Robot_->generate_trajectories( time, Solution_,
+        deque<FootAbsolutePosition> LeftFootTraj (FinalLeftFootTraj_deq);
+        deque<FootAbsolutePosition> RightFootTraj (FinalRightFootTraj_deq);
+        Robot_->generate_trajectories( time + i * QP_T_ , Solution_,
             Solution_.SupportStates_deq, Solution_.SupportOrientations_deq,
-            LeftFootTraj_deq, RightFootTraj_deq );
+            LeftFootTraj, RightFootTraj );
+
+        for (unsigned int k = 0 ; k < numberOfSample ; k++)
+        {
+          LeftFootTraj_deq[currentIndex + i * numberOfSample + k] = LeftFootTraj[currentIndex + k] ;
+          RightFootTraj_deq[currentIndex + i * numberOfSample + k] = RightFootTraj[currentIndex + k] ;
+        }
       }
 
       // DYNAMIC FILTER
       // --------------
       //DynamicFilter( FinalZMPTraj_deq, FinalCOMTraj_deq, FinalLeftFootTraj_deq, FinalRightFootTraj_deq );
 
+      FinalCOMTraj_deq.resize( numberOfSample + currentIndex );
+      FinalZMPTraj_deq.resize( numberOfSample + currentIndex );
+      FinalLeftFootTraj_deq.resize( numberOfSample + currentIndex );
+      FinalRightFootTraj_deq.resize( numberOfSample + currentIndex );
+
       for (unsigned int i = 0 ; i < FinalZMPTraj_deq.size() ; i++ )
       {
         FinalZMPTraj_deq[i] = ZMPTraj_deq[i] ;
@@ -510,11 +521,10 @@ ZMPVelocityReferencedQP::OnLine(double time,
         FinalRightFootTraj_deq[i] = RightFootTraj_deq[i] ;
       }
 
-
       // Specify that we are in the ending phase.
       if (EndingPhase_ == false)
       {
-          TimeToStopOnLineMode_ = UpperTimeLimitToUpdate_ + QP_T_ * QP_N_;
+        TimeToStopOnLineMode_ = UpperTimeLimitToUpdate_ + QP_T_ * QP_N_;
       }
       UpperTimeLimitToUpdate_ = UpperTimeLimitToUpdate_ + QP_T_;