diff --git a/CMakeLists.txt b/CMakeLists.txt
index f3f91d5a49bedc2e7760d4d0c19b5a5f3a71647b..2b6c002793fd591e339013f0e4b4c44a79b99ebf 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -50,10 +50,6 @@ ADD_REQUIRED_DEPENDENCY("jrl-dynamics >= 1.19.1")
 ADD_OPTIONAL_DEPENDENCY("hrp2-dynamics >= 1.1.0")
 
 # Add aggressive optimization flags in release mode.
-IF(CMAKE_COMPILER_IS_GNUCXX)
-  SET (CMAKE_CXX_FLAGS_RELEASE
-    "-O2 -DNDEBUG")
-ENDIF(CMAKE_COMPILER_IS_GNUCXX)
 
 #IF(CMAKE_COMPILER_IS_GNUCXX)
 #  SET (CMAKE_CXX_FLAGS_RELEASE
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
index 7ef59cfae1ee44c89fcbe87a33d5177e27b47b73..a1cf26458a0e52f6a71d07ba4b98cf90ddd3946e 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
@@ -166,9 +166,8 @@ QPProblem_s::solve( int solver, solution_t & result )
       //      else
       iwar_.array_[0]=1;
 
-      Q_.stick_together(Q_dense_.array_,n_,n_);
-      DU_.stick_together(DU_dense_.array_,mmax_,n_);
-
+      Q_.stick_together(Q_dense_,n_,n_);
+      DU_.stick_together(DU_dense_,mmax_,n_);
       
       ql0001_(&m_, &me_, &mmax_, &n_, &nmax_, &mnn_,
               Q_dense_.array_, D_.array_, DU_dense_.array_, DS_.array_, XL_.array_, XU_.array_,
@@ -177,7 +176,6 @@ QPProblem_s::solve( int solver, solution_t & result )
 
       result.resize(n_,m_);
 
-
       for(int i = 0; i < n_; i++)
         {
           result.Solution_vec(i) = X_.array_[i];
@@ -223,20 +221,6 @@ QPProblem_s::add_term( const MAL_MATRIX (&Mat, double), int type,
       break;
     }
 
-  /*  std::cout << "NbVariables_:" << NbVariables_
-	    << " NbConstraints_:" << NbConstraints_ 
-	    << " NbEqConstraints_: " << NbEqConstraints_
-	    << " m_ :"     << m_
-	    << " me_:"     << me_
-	    << " mmax_: "  << mmax_
-	    << " n_:"      << n_
-	    << " nmax_: "  << nmax_
-	    << " mnn_: "   << mnn_ 
-	    << " lwar_:"   << lwar_
-	    << " liwar_ :" << liwar_ 
-	    << std::endl; */
-  if (NbEqConstraints_>0)
-    std::cout << "NbEqConstraints:" << NbEqConstraints_<< std::endl;
 
   if (NbVariables_ > pArray_s->ncols_ )
     {
@@ -257,13 +241,13 @@ QPProblem_s::add_term( const MAL_MATRIX (&Mat, double), int type,
       DS_.resize(2*NbConstraints_,1,true);
     }
   
-  unsigned long int Usize = 2*(NbConstraints_+2*NbVariables_);
+  int Usize = 2*(NbConstraints_+2*NbVariables_);
   if (Usize>U_.nrows_)
     {
       U_.resize(Usize, 1,true);
     }
   
-  unsigned long int warsize = 2*(3*NbVariables_*NbVariables_/2+10*NbVariables_+2*(NbConstraints_+1)+20000);
+  int warsize = 2*(3*NbVariables_*NbVariables_/2+10*NbVariables_+2*(NbConstraints_+1)+20000);
   
   if (warsize> war_.nrows_)
     {
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.hh b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
index c5da55126319ae514b0ee6ec50b34cdb0dd24a59..8bbe6607c163c57154085e6dfd2686ecc47f3017 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.hh
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
@@ -221,6 +221,7 @@ namespace PatternGeneratorJRL
 
       int id_;
       int nrows_, ncols_;
+      unsigned int memsize_;
 
       void fill( type value)
       { std::fill_n(array_, nrows_*ncols_, value); }
@@ -235,18 +236,24 @@ namespace PatternGeneratorJRL
       /// \param[in] ncols Size of the new array
       /// \param[in] preserve Preserve old values
       /// \return 0
-      int stick_together(type *& final_array, int nrows, int ncols)
+      int stick_together(struct array_s<type> & final_array, 
+			 int nrows, int ncols)
       {
         try {
-          type * NewArray = new type[nrows*ncols];
+	  type * NewArray = 0;
+	  if ((final_array.memsize_>nrows*ncols) ||
+	      (final_array.array_==0))
+	    {
+	      final_array.array_ = new type[nrows*ncols];
+	      final_array.memsize_ = nrows*ncols;
+	    }
+	  NewArray = final_array.array_;
+	  
           fill(NewArray, nrows*ncols, (type)0);
           for(int i = 0; i < nrows; i++)
             for(int j = 0; j < ncols; j++)
               NewArray[i+nrows*j] = array_[i+nrows_*j];
-          if (final_array!=0)
-            delete [] final_array;
 
-          final_array = NewArray;
           nrows_ = nrows;
           ncols_ = ncols;
         }
@@ -265,18 +272,30 @@ namespace PatternGeneratorJRL
       int resize(int nrows, int ncols, bool preserve)
       {
         try {
-
-          type * NewArray = new type[nrows*ncols];
-          fill(NewArray, nrows*ncols, (type)0);
-          if ((preserve) && 
+	  bool b_reallocate = false;
+	  type * NewArray = 0;
+	  if (nrows*ncols>memsize_)
+	    {
+	      NewArray = new type[nrows*ncols];
+	      memsize_ = nrows*ncols;
+	      b_reallocate = true;
+	      std::cout << "memsize_ " << memsize_ << std::endl;
+	    }
+	  else NewArray = array_;
+
+	  fill(NewArray, nrows*ncols, (type)0);
+	  if ((preserve) && 
 	      (array_!=0) ) {
-            for(int i = 0; i < nrows_; i++)
-              for(int j = 0; j < ncols_; j++)
-                NewArray[i+nrows*j] = array_[i+nrows_*j]; }
-          if (array_!=0)
-            delete [] array_;
-
-          array_ = NewArray;
+	    for(int i = 0; i < nrows_; i++)
+	      for(int j = 0; j < ncols_; j++)
+		NewArray[i+nrows*j] = array_[i+nrows_*j]; }
+
+	  if ((array_!=0) && b_reallocate)
+	    {
+	      delete [] array_;
+	    }
+	  array_ = NewArray;
+	  
           nrows_ = nrows;
           ncols_ = ncols;
         }
@@ -287,7 +306,7 @@ namespace PatternGeneratorJRL
       }
 
       array_s():
-        array_(0),id_(0),nrows_(0),ncols_(0){
+        array_(0),id_(0),nrows_(0),ncols_(0), memsize_(0){
       };
       ~array_s()
       {  if (array_!=0) delete [] array_;};