From 6889eccf768fb9db05e55e212cc5bdd40fae95b7 Mon Sep 17 00:00:00 2001
From: olivier-stasse <olivier.stasse@aist.go.jp>
Date: Fri, 11 Mar 2011 05:07:26 +0900
Subject: [PATCH] Revert "Add debug methods"
This reverts commit 64342f7cbe05bc80205f50c39bfcec50d3acad32.
---
src/Mathematics/intermediate-qp-matrices.cpp | 52 +++----------------
src/Mathematics/intermediate-qp-matrices.hh | 18 ++-----
.../generator-vel-ref.cpp | 28 +++++-----
.../generator-vel-ref.hh | 5 ++
4 files changed, 30 insertions(+), 73 deletions(-)
diff --git a/src/Mathematics/intermediate-qp-matrices.cpp b/src/Mathematics/intermediate-qp-matrices.cpp
index f9c6a2ef..030fca43 100644
--- a/src/Mathematics/intermediate-qp-matrices.cpp
+++ b/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();
}
diff --git a/src/Mathematics/intermediate-qp-matrices.hh b/src/Mathematics/intermediate-qp-matrices.hh
index 509b674b..1dcf7acc 100644
--- a/src/Mathematics/intermediate-qp-matrices.hh
+++ b/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);
+
};
}
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index 093b99de..f48171ac 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/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);
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
index 793fdfa0..b85e05a4 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+++ b/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;
--
GitLab