diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
index 7b32ff309a18c1224518d887815e43ad26057355..732931b87af452640809f054decd691b242f26c7 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
@@ -897,9 +897,9 @@ bool ComAndFootRealizationByGeometry::
                                             int IterationNumber,
                                             int Stage)
 {
+  MAL_S3_VECTOR(AbsoluteWaistPosition,double);
   MAL_VECTOR_DIM(lqr,double,6);
   MAL_VECTOR_DIM(lql,double,6);
-  MAL_S3_VECTOR(AbsoluteWaistPosition,double);
 
   // Kinematics for the legs.
   KinematicsForTheLegs(aCoMPosition,
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index ea1326ad13a8e414ca87209a5890283587f83bc4..36e49e51e5b702b17d228d5f10a6ffd00a8d7976 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -29,10 +29,6 @@ DynamicFilter::DynamicFilter(
       SPM,OptimalControllerSolver::MODE_WITH_INITIALPOS,false);
   CoMHeight_ = 0.0 ;
 
-  configurationTraj_.clear();
-  velocityTraj_.clear();
-  accelerationTraj_.clear();
-  previousConfiguration_.clear();
   deltaZMP_deq_.clear();
   ZMPMB_vec_.clear();
 
@@ -44,14 +40,10 @@ DynamicFilter::DynamicFilter(
   MAL_MATRIX_RESIZE(deltax_,3,1);
   MAL_MATRIX_RESIZE(deltay_,3,1);
 
-  previousConfiguration_ = aHS->currentConfiguration() ;
-  previousVelocity_ = aHS->currentVelocity() ;
-  previousAcceleration_ = aHS->currentAcceleration() ;
-
   comAndFootRealization_->SetPreviousConfigurationStage0(
-      previousConfiguration_);
+      aHS->currentConfiguration());
   comAndFootRealization_->SetPreviousVelocityStage0(
-      previousVelocity_);
+      aHS->currentVelocity());
 
   Once_ = true ;
   DInitX_ = 0.0 ;
@@ -140,10 +132,6 @@ void DynamicFilter::init(
   PC_->SetHeightOfCoM(CoMHeight_);
   PC_->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITH_INITIALPOS);
 
-  previousConfiguration_ = comAndFootRealization_->getHumanoidDynamicRobot()->currentConfiguration() ;
-  previousVelocity_ = comAndFootRealization_->getHumanoidDynamicRobot()->currentVelocity() ;
-  previousAcceleration_ = comAndFootRealization_->getHumanoidDynamicRobot()->currentAcceleration() ;
-
   upperPartConfiguration_ = comAndFootRealization_->getHumanoidDynamicRobot()->currentConfiguration() ;
   previousUpperPartConfiguration_ = comAndFootRealization_->getHumanoidDynamicRobot()->currentConfiguration() ;
   upperPartVelocity_ = comAndFootRealization_->getHumanoidDynamicRobot()->currentVelocity() ;
@@ -154,10 +142,6 @@ void DynamicFilter::init(
   ZMPMBVelocity_ = comAndFootRealization_->getHumanoidDynamicRobot()->currentVelocity() ;
   ZMPMBAcceleration_ = comAndFootRealization_->getHumanoidDynamicRobot()->currentAcceleration() ;
 
-  configurationTraj_.resize( PG_N_, previousConfiguration_ ); ;
-  velocityTraj_.resize( PG_N_, previousVelocity_ ); ;
-  accelerationTraj_.resize( PG_N_, previousAcceleration_ ); ;
-
   for(unsigned int j = 0 ; j < ZMPMBConfiguration_.size() ; j++ )
   {
     q_(j,0) = ZMPMBConfiguration_(j) ;
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index d0552927701890e21d65100d17de83b4bbc6b1ff..f7377dd12c55b6046261b9e76b1638cd68542797 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -214,12 +214,6 @@ namespace PatternGeneratorJRL
       ComAndFootRealizationByGeometry * comAndFootRealization_;
 
       /// \brief Buffers for the Inverse Kinematics
-      std::vector <MAL_VECTOR_TYPE(double)> configurationTraj_ ;
-      std::vector <MAL_VECTOR_TYPE(double)> velocityTraj_ ;
-      std::vector <MAL_VECTOR_TYPE(double)> accelerationTraj_ ;
-      MAL_VECTOR_TYPE(double) previousConfiguration_ ;
-      MAL_VECTOR_TYPE(double) previousVelocity_ ;
-      MAL_VECTOR_TYPE(double) previousAcceleration_ ;
       MAL_VECTOR_TYPE(double) aCoMState_;
       MAL_VECTOR_TYPE(double) aCoMSpeed_;
       MAL_VECTOR_TYPE(double) aCoMAcc_;
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 372ea4e5844213e94280f118f6650295b25a7c86..321a3cd1ee51e719f438aa0e38a6fff055ebd2d2 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -190,7 +190,8 @@ Solution_(),OFTG_DF_(0),OFTG_control_(0),dynamicFilter_(0)
   string aMethodName[NbMethods] =
   {":previewcontroltime",
    ":numberstepsbeforestop",
-   ":stoppg"};
+   ":stoppg"
+   ":setfeetconstraint"};
 
   for(unsigned int i=0;i<NbMethods;i++)
   {
@@ -307,9 +308,11 @@ void ZMPVelocityReferencedQP::CallMethod(std::string & Method, std::istringstrea
   {
     EndingPhase_ = true;
   }
-
+  if(Method==":setfeetconstraint")
+  {
+   RFI_->CallMethod(Method,strm);
+  }
   ZMPRefTrajectoryGeneration::CallMethod(Method,strm);
-
 }
 
 int
@@ -854,6 +857,12 @@ void
           << filterprecision( FinalZMPTraj_deq[i].py ) << " "       // 54
           << filterprecision( filteredZMPMB[i][0] ) << " "       // 55
           << filterprecision( filteredZMPMB[i][1] ) << " "       // 56
+          << filterprecision( outputDeltaCOMTraj_deq[i].x[0] ) << " "       // 57
+          << filterprecision( outputDeltaCOMTraj_deq[i].x[1] ) << " "       // 58
+          << filterprecision( outputDeltaCOMTraj_deq[i].x[2] ) << " "       // 59
+          << filterprecision( outputDeltaCOMTraj_deq[i].y[0] ) << " "       // 60
+          << filterprecision( outputDeltaCOMTraj_deq[i].y[1] ) << " "       // 61
+          << filterprecision( outputDeltaCOMTraj_deq[i].y[2] ) << " "       // 62
           << endl ;
     }
     aof.close();