Revert "Add debug methods"

This reverts commit 64342f7c.

src/Mathematics/intermediate-qp-matrices.cpp

@@ -27,7 +27,8 @@
 
 
 
-#include <Mathematics/intermediate-qp-matrices.hh>
+#include <Mathematics/intermediate-qp-matrices.hh> //TODO: Merge with ZMPVelocityReferencedQPDebug.cpp
+
 
 using namespace PatternGeneratorJRL;
 
@@ -51,7 +52,7 @@ IntermedQPMat::~IntermedQPMat()
 
 
 IntermedQPMat::objective_variant_t const &
-IntermedQPMat::operator ()( const int aObjectiveType ) const
+IntermedQPMat::operator ()( int aObjectiveType ) const
 {
   switch(aObjectiveType)
   {
@@ -68,7 +69,7 @@ IntermedQPMat::operator ()( const int aObjectiveType ) const
 
 
 IntermedQPMat::objective_variant_t &
-IntermedQPMat::operator ()( const int aObjectiveType )
+IntermedQPMat::operator ()( int aObjectiveType )
 {
   switch(aObjectiveType)
   {
@@ -84,56 +85,15 @@ IntermedQPMat::operator ()( const int aObjectiveType )
 }
 
 
-void IntermedQPMat::printObjective( const int ObjectiveType, std::ostream &aos )
-{
-  IntermedQPMat::objective_variant_t objective;
-  switch(ObjectiveType)
-    {
-    case INSTANT_VELOCITY:
-      objective = m_InstantVelocity;
-      aos << "Instant velocity - " <<ObjectiveType<<" : "<<std::endl;
-    case JERK:
-      objective = m_Jerk;
-      aos << "Jerk: " <<std::endl;
-      aos << "Instant velocity: - " <<ObjectiveType<<" : "<<std::endl;
-    case COP_CENTERING:
-      objective = m_COPCentering;
-      aos << "COP centering: " <<std::endl;
-      aos << "Instant velocity: - " <<ObjectiveType<<" : "<<std::endl;
-    }
-  aos << "Ponderation: " << std::endl;
-  aos << objective.weight << std::endl;
-  aos << "U: " << std::endl;
-  aos << objective.U << std::endl;
-  aos << "trans(U): " << std::endl;
-  aos << objective.UT << std::endl;
-  aos << "S: " << std::endl;
-  aos << objective.S << std::endl;
-}
-
-
-void IntermedQPMat::printState( std::ostream &aos )
+void IntermedQPMat::printObjective( int ObjectiveType, std::ostream &aos )
 {
 
 }
 
 
-void IntermedQPMat::printObjective(const char * filename,
-                          const int type)
+void IntermedQPMat::printState( std::ostream &aos )
 {
-  std::ofstream aof;
-  aof.open(filename,std::ofstream::out);
-  printObjective(type,aof);
-  aof.close();
-}
-
 
-void IntermedQPMat::printState(const char * filename)
-{
-  std::ofstream aof;
-  aof.open(filename,std::ofstream::out);
-  printState(aof);
-  aof.close();
 }
 
 

src/Mathematics/intermediate-qp-matrices.hh

@@ -31,10 +31,9 @@
 //# include <boost/numeric/ublas/matrix.hpp>
 //# include <boost/numeric/ublas/vector.hpp>
 #include <jrl/mal/matrixabstractlayer.hh>
+
 #include <privatepgtypes.h>
 
-#include <iostream>
-#include <fstream>
 
 namespace PatternGeneratorJRL
 {
@@ -47,11 +46,6 @@ namespace PatternGeneratorJRL
     //
   public:
 
-    const static int MEAN_VELOCITY = 0;
-    const static int INSTANT_VELOCITY = 1;
-    const static int COP_CENTERING = 2;
-    const static int JERK = 3;
-
     /// \name QP elements that are objective independent
     /// \{
     struct state_variant_s
@@ -114,8 +108,8 @@ namespace PatternGeneratorJRL
     { return m_StateMatrices; };
 
     /// \brief Accessors to the objective dependent matrices
-    objective_variant_t const & operator ()( const int aObjectiveType ) const;
-    objective_variant_t & operator ()( const int aObjectiveType );
+    objective_variant_t const & operator ()( int aObjectiveType ) const;
+    objective_variant_t & operator ()( int aObjectiveType );
 
     /// \brief Accessors to the Center of Mass
     //inline com_t const & operator ()()
@@ -124,11 +118,8 @@ namespace PatternGeneratorJRL
     { m_StateMatrices.CoM = CoM; };
 
     /// \brief Printers
-    void printObjective( const int ObjectiveType, std::ostream &aos );
+    void printObjective( int ObjectiveType, std::ostream &aos );
     void printState( std::ostream &aos );
-    void printObjective(const char * filename, const int Objectivetype);
-    void printState(const char * filename);
-
 
 
     //
@@ -158,6 +149,7 @@ namespace PatternGeneratorJRL
     MAL_MATRIX(m_OptC,double);
     MAL_MATRIX(m_OptD,double);
 
+
   };
 }
 

src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp

@@ -52,7 +52,7 @@ GeneratorVelRef::CallMethod(std::string &Method, std::istringstream &strm)
 
 	
 void 
-GeneratorVelRef::setPonderation( IntermedQPMat Matrices, double weight, const int aObjectiveType)
+GeneratorVelRef::setPonderation( IntermedQPMat Matrices, double weight, int aObjectiveType)
 {
 
   IntermedQPMat::objective_variant_t & Objective = Matrices( aObjectiveType );
@@ -79,11 +79,11 @@ void
 GeneratorVelRef::initializeProblem(QPProblem & Pb, IntermedQPMat Matrices)
 {
 
-  IntermedQPMat::objective_variant_t & InstVel = Matrices( IntermedQPMat::INSTANT_VELOCITY );
+  IntermedQPMat::objective_variant_t InstVel = Matrices( INSTANT_VELOCITY );
   initializeMatrices( InstVel );
-  IntermedQPMat::objective_variant_t & COPCent = Matrices( IntermedQPMat::COP_CENTERING );
+  IntermedQPMat::objective_variant_t COPCent = Matrices( COP_CENTERING );
   initializeMatrices( COPCent );
-  IntermedQPMat::objective_variant_t & Jerk = Matrices( IntermedQPMat::JERK );
+  IntermedQPMat::objective_variant_t Jerk = Matrices( JERK );
   initializeMatrices( Jerk );
 
 }
@@ -99,7 +99,7 @@ GeneratorVelRef::initializeMatrices( IntermedQPMat::objective_variant_t & Object
 
   switch(Objective.type)
   {
-    case IntermedQPMat::MEAN_VELOCITY || IntermedQPMat::INSTANT_VELOCITY:
+    case MEAN_VELOCITY || INSTANT_VELOCITY:
     for(int i=0;i<m_N;i++)
       {
         Objective.S(i,0) = 0.0; Objective.S(i,1) = 1.0; Objective.S(i,2) = (i+1)*m_T_Prw;
@@ -109,7 +109,7 @@ GeneratorVelRef::initializeMatrices( IntermedQPMat::objective_variant_t & Object
             else
                 Objective.U(i,j) = 0.0;
       }
-    case IntermedQPMat::COP_CENTERING:
+    case COP_CENTERING:
       for(int i=0;i<m_N;i++)
         {
           Objective.S(i,0) = 1.0; Objective.S(i,1) = (i+1)*m_T_Prw; Objective.S(i,2) = (i+1)*(i+1)*m_T_Prw*m_T_Prw*0.5-m_CoMHeight/9.81;
@@ -119,7 +119,7 @@ GeneratorVelRef::initializeMatrices( IntermedQPMat::objective_variant_t & Object
               else
                   Objective.U(i,j) = 0.0;
         }
-    case IntermedQPMat::JERK:
+    case JERK:
       for(int i=0;i<m_N;i++)
         {
           Objective.S(i,0) = 0.0; Objective.S(i,1) = 0.0; Objective.S(i,2) = 0.0;
@@ -176,13 +176,13 @@ void
 GeneratorVelRef::buildInvariantProblemPart(QPProblem & Pb, IntermedQPMat & Matrices)
 {
 
-  const IntermedQPMat::objective_variant_t & Jerk = Matrices(IntermedQPMat::JERK);
+  const IntermedQPMat::objective_variant_t & Jerk = Matrices(JERK);
   updateProblem(Pb.Q, Jerk);
 
-  const IntermedQPMat::objective_variant_t & InstVel = Matrices(IntermedQPMat::INSTANT_VELOCITY);
+  const IntermedQPMat::objective_variant_t & InstVel = Matrices(INSTANT_VELOCITY);
   updateProblem(Pb.Q, InstVel);
 
-  const IntermedQPMat::objective_variant_t & COPCent = Matrices(IntermedQPMat::COP_CENTERING);
+  const IntermedQPMat::objective_variant_t & COPCent = Matrices(COP_CENTERING);
   updateProblem(Pb.Q, COPCent);
 
 }
@@ -192,11 +192,11 @@ void
 GeneratorVelRef::updateProblem(QPProblem & Pb, IntermedQPMat & Matrices)
 {
 
-  const IntermedQPMat::objective_variant_t & InstVel = Matrices(IntermedQPMat::INSTANT_VELOCITY);
+  const IntermedQPMat::objective_variant_t & InstVel = Matrices(INSTANT_VELOCITY);
   const IntermedQPMat::state_variant_t & State = Matrices();
   updateProblem(Pb.Q, Pb.D, InstVel, State);
 
-  const IntermedQPMat::objective_variant_t & COPCent = Matrices(IntermedQPMat::COP_CENTERING);
+  const IntermedQPMat::objective_variant_t & COPCent = Matrices(COP_CENTERING);
   updateProblem(Pb.Q, Pb.D, COPCent, State);
 
 }
@@ -233,7 +233,7 @@ GeneratorVelRef::updateProblem(double * Q, double *p,
   // Quadratic part of the Objective
   switch(Objective.type)
   {
-  case IntermedQPMat::COP_CENTERING:
+  case COP_CENTERING:
     // Quadratic part of the objective
     computeTerm(weightMTM, -Objective.weight, Objective.UT, State.V);
     addTerm(weightMTM, Q, 0, 2*m_N, m_N, State.NbStepsPrw);
@@ -254,7 +254,7 @@ GeneratorVelRef::updateProblem(double * Q, double *p,
     addTerm(weightMTV, p, 2*m_N, State.NbStepsPrw);
     computeTerm(weightMTV, Objective.weight,Objective.UT,State.Vc,State.fx);
     addTerm(weightMTV, p, 2*m_N+State.NbStepsPrw, State.NbStepsPrw);
-  case IntermedQPMat::INSTANT_VELOCITY:
+  case INSTANT_VELOCITY:
     // Linear part of the objective
     computeTerm(weightMTV, Objective.weight,Objective.UT, MV, Objective.S, State.CoM.x);
     addTerm(weightMTV, p, 0, m_N);

src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh

@@ -208,6 +208,11 @@ namespace PatternGeneratorJRL
     //Private members
     //
   private:
+
+    const static int MEAN_VELOCITY = 0;
+    const static int INSTANT_VELOCITY = 1;
+    const static int COP_CENTERING = 2;
+    const static int JERK = 3;
 	  
     /// \brief The current and the previewed support state.
     SupportState_t m_CurrentSupport, m_PrwSupport;