Revert "Assure proper initialization of matrices"

This reverts commit 3b1c4b4a.

src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp

@@ -100,17 +100,11 @@ GeneratorVelRef::generateSelectionMatrices(IntermedQPMat & Matrices,
 
   IntermedQPMat::state_variant_t & State = Matrices.State();
   const int & NbPrwSteps = deqSupportStates.back().StepNumber;
-
   MAL_VECTOR_RESIZE(State.Vc,m_N);
-  State.Vc.clear();
   MAL_MATRIX_RESIZE(State.V,m_N,NbPrwSteps);
-  MAL_MATRIX_CLEAR(State.V);
-  MAL_MATRIX_RESIZE(State.VT,NbPrwSteps,m_N);
-  MAL_MATRIX_CLEAR(State.VT);
 
   std::deque<support_state_t>::iterator SS_it;
-  SS_it = deqSupportStates.begin();//points at the cur. sup. st.
-
+  SS_it = deqSupportStates.begin();
   SS_it++;
   for(int i=0;i<m_N;i++)
     {
@@ -131,9 +125,7 @@ GeneratorVelRef::computeGlobalReference(IntermedQPMat & Matrices, COMState Trunk
   reference_t & Ref = Matrices.Reference();
 
   MAL_VECTOR_RESIZE(Ref.global.X,m_N);
-  Ref.global.X.clear();
   MAL_VECTOR_RESIZE(Ref.global.Y,m_N);
-  Ref.global.Y.clear();
 
   for( int i=0;i<m_N;i++)
     {
@@ -165,11 +157,8 @@ GeneratorVelRef::initializeMatrices( IntermedQPMat::objective_variant_t & Object
 {
 
   MAL_MATRIX_RESIZE(Objective.U,m_N,m_N);
-  MAL_MATRIX_CLEAR(Objective.U);
   MAL_MATRIX_RESIZE(Objective.UT,m_N,m_N);
-  MAL_MATRIX_CLEAR(Objective.UT);
   MAL_MATRIX_RESIZE(Objective.S,m_N,3);
-  MAL_MATRIX_CLEAR(Objective.S);
 
   switch(Objective.type)
     {
@@ -311,6 +300,7 @@ GeneratorVelRef::updateProblem(QPProblem & Pb, IntermedQPMat & Matrices,
   Pb.addTerm(weightMTM, QPProblem::MATRIX_Q, 2*m_N, 2*m_N);
   Pb.addTerm(weightMTM, QPProblem::MATRIX_Q, 2*m_N+deqSupportStates[m_N].StepNumber, 2*m_N+deqSupportStates[m_N].StepNumber);
 
+  // Linear part of the objective
   computeTerm(weightMTV, -COPCent.weight, State.VT, MV, COPCent.S, State.CoM.x);
   Pb.addTerm(weightMTV, QPProblem::VECTOR_D, 2*m_N);
   computeTerm(weightMTV, -COPCent.weight, State.VT, MV, COPCent.S, State.CoM.y);