From 7fea6fe30761e110f4527e58386aa1eb2734b50c Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Wed, 10 Feb 2016 14:31:41 +0100
Subject: [PATCH] begin the intergration of the interpolation. still work to do

---
 .../ZMPVelocityReferencedSQP.cpp              | 10 +++----
 .../nmpc_generator.cpp                        | 28 +++++++++++--------
 .../nmpc_generator.hh                         |  5 ++++
 3 files changed, 26 insertions(+), 17 deletions(-)

diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp
index b0992e92..0db709f9 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp
@@ -379,8 +379,8 @@ void ZMPVelocityReferencedSQP::OnLine(double time,
 
   // UPDATE WALKING TRAJECTORIES:
   // ----------------------------
-  if(time + 0.00001 > UpperTimeLimitToUpdate_)
-  {
+  //if(time + 0.00001 > UpperTimeLimitToUpdate_)
+  //{
     // UPDATE INTERNAL DATA:
     // ---------------------
     if(PerturbationOccured_ &&
@@ -433,7 +433,7 @@ void ZMPVelocityReferencedSQP::OnLine(double time,
       FinalRightFootTraj_deq[i] = RightFootTraj_deq_ctrl_[i] ;
     }
 
-    bool filterOn_ = true ;
+    bool filterOn_ = false ;
     if(filterOn_)
     {
 
@@ -482,7 +482,7 @@ void ZMPVelocityReferencedSQP::OnLine(double time,
         }
         UpperTimeLimitToUpdate_ = UpperTimeLimitToUpdate_ + SQP_T_;
       }
-  }
+  //}
   //-----------------------------------
   //
   //
@@ -513,7 +513,7 @@ void ZMPVelocityReferencedSQP::FullTrajectoryInterpolation(double time)
   LIPM_.setState(itCOM_);
 
   CoMZMPInterpolation(JerkX,JerkY,&LIPM_,NbSampleControl_,0,CurrentIndex_,SupportStates_deq);
-  itCOM_ = LIPM_.GetState();
+  itCOM_ = COMTraj_deq_ctrl_[1];
 
   support_state_t currentSupport = SupportStates_deq[0] ;
   currentSupport.StepNumber=0;
diff --git a/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp b/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
index 0f35ccd8..5a0832cb 100644
--- a/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
+++ b/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
@@ -93,6 +93,7 @@ NMPCgenerator::NMPCgenerator(SimplePluginManager * aSPM, CjrlHumanoidDynamicRobo
 {
   time_=0.0;
   T_ = 0.0 ;
+  Tfirst_ = 0.0 ;
   N_ = 0 ;
   nf_ = 0 ;
   T_step_ = 0.0 ;
@@ -190,6 +191,7 @@ void NMPCgenerator::initNMPCgenerator(support_state_t & currentSupport,
   MAL_VECTOR_RESIZE(ubB0ds_,4) ;                 MAL_VECTOR_FILL(ubB0ds_,0.0);
 
   T_ = T ;
+  Tfirst_ = T ;
   T_step_ = T_step ;
   alpha_ = 5   ;// 1     ; // weight for CoM velocity tracking  : 0.5 * a ; 2.5
   beta_  = 1e+03 ;// 1     ; // weight for ZMP reference tracking : 0.5 * b ; 1e+03
@@ -226,8 +228,8 @@ void NMPCgenerator::initNMPCgenerator(support_state_t & currentSupport,
   FeetDistance_ = RFI_->DSFeetDistance();
 
   // build constant matrices
-  buildCoMIntegrationMatrix(0.1);
-  buildCoPIntegrationMatrix(0.1);
+  buildCoMIntegrationMatrix(Tfirst_);
+  buildCoPIntegrationMatrix(Tfirst_);
   buildConvexHullSystems();
 
   // initialize time dependant matrices
@@ -292,18 +294,20 @@ void NMPCgenerator::updateInitialCondition(double time,
 //       << currentSupport_.StartTime << "  "
 //       << currentSupport_.TimeLimit << "  "
 //       << endl ;
-  double T = 0.0;
   if(currentSupport_.Phase==DS)
-    T=0.1;
+    Tfirst_=0.1;
   else
-    T = (time_-currentSupport_.StartTime)
+  {
+    Tfirst_ = (time_-currentSupport_.StartTime)
         - ((double)(int)((time_-currentSupport_.StartTime)/0.1) * 0.1) ;
-  if(T<0.0001)
-    T=0.1;
+    Tfirst_ = 0.1 - Tfirst_;
+  }
+  if(Tfirst_<0.0001)
+    Tfirst_=0.1;
 
-  cout << T << endl ;
-  buildCoPIntegrationMatrix(T);
-  buildCoMIntegrationMatrix(T);
+  cout << Tfirst_ << endl ;
+  buildCoPIntegrationMatrix(Tfirst_);
+  buildCoMIntegrationMatrix(Tfirst_);
 
   return ;
 }
@@ -588,10 +592,10 @@ void NMPCgenerator::computeFootSelectionMatrix()
   DumpMatrix("V_kp1_",V_kp1_);
   DumpVector("v_kp1_",v_kp1_);
 #endif
-#ifdef DEBUG_COUT
+//#ifdef DEBUG_COUT
   cout << "V_kp1_ = " << V_kp1_ << endl ;
   cout << "v_kp1_ = " << v_kp1_ << endl ;
-#endif
+//#endif
   return ;
 }
 
diff --git a/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh b/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh
index 0190b41a..d4dcb9a7 100644
--- a/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh
+++ b/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh
@@ -158,6 +158,9 @@ namespace PatternGeneratorJRL
     inline void T(double T)
     {T_=T;}
 
+    inline double Tfirst()
+    {return Tfirst_ ;}
+
     // Number of sample in the SQP preview
     inline double N()
     {return N_;}
@@ -284,6 +287,8 @@ namespace PatternGeneratorJRL
 
     // Sampling period of the SQP preview
     double T_ ;
+    // Sampling period of the first SQP preview iteration
+    double Tfirst_ ;
     // Number of sample in the SQP preview
     unsigned N_ ;
     // Number of foot step planned in the preview
-- 
GitLab