Commit def5f182 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Remove debugging information back to 3 ms of computation.

parent addb5dcc
......@@ -100,7 +100,7 @@ FootTrajectoryGenerationStandard
m_AnklePositionLeft[1] = -lWidth*0.5 + AnklePosition(1);
m_AnklePositionLeft[2] = AnklePosition(2);
RESETDEBUG5("GeneratedFoot.dat");
RESETDEBUG4("GeneratedFoot.dat");
}
FootTrajectoryGenerationStandard::
......
......@@ -63,24 +63,24 @@ ComAndFootRealizationByGeometry::
{ for(unsigned int i=0;i<m_prev_Velocity1.size();m_prev_Velocity1[i++]=0.0);};
{ for(unsigned int i=0;i<m_prev_Velocity2.size();m_prev_Velocity2[i++]=0.0);};
RESETDEBUG5("DebugDataVelocity.dat");
RESETDEBUG4("DebugDataVelocity.dat");
RESETDEBUG5("LegsSpeed.dat");
RESETDEBUG5("COMPC1.dat");
RESETDEBUG5("DebugDataIK.dat");
RESETDEBUG5("DebugDatamDtL.dat");
RESETDEBUG5("DebugDataStartingCOM.dat");
RESETDEBUG4("LegsSpeed.dat");
RESETDEBUG4("COMPC1.dat");
RESETDEBUG4("DebugDataIK.dat");
RESETDEBUG4("DebugDatamDtL.dat");
RESETDEBUG4("DebugDataStartingCOM.dat");
RESETDEBUG5("DebugDataCOMForHeuristic.txt");
RESETDEBUG5("DebugDataIKArms.txt");
RESETDEBUG5("DebugDataqArmsHeuristic.txt");
RESETDEBUG5("DebugDataVelocity0.dat");
RESETDEBUG5("DebugDataVelocity1.dat");
RESETDEBUG4("DebugDataCOMForHeuristic.txt");
RESETDEBUG4("DebugDataIKArms.txt");
RESETDEBUG4("DebugDataqArmsHeuristic.txt");
RESETDEBUG4("DebugDataVelocity0.dat");
RESETDEBUG4("DebugDataVelocity1.dat");
RESETDEBUG5("DebugDataBodyP0.dat");
RESETDEBUG5("DebugDataBodyP1.dat");
RESETDEBUG5("FootPosRight.dat");
RESETDEBUG5("FootPosLeft.dat");
RESETDEBUG4("DebugDataBodyP0.dat");
RESETDEBUG4("DebugDataBodyP1.dat");
RESETDEBUG4("FootPosRight.dat");
RESETDEBUG4("FootPosLeft.dat");
// Variables for stepping over upper body motion.
m_UpperBodyMotion.resize(3);
......@@ -459,7 +459,7 @@ InitializationCoM
<< m_DiffBetweenComAndWaist[2]);
// This term is usefull if
ODEBUG5("m_DiffBetweenComAndWaist :" << m_DiffBetweenComAndWaist,"DebugData.txt");
ODEBUG4("m_DiffBetweenComAndWaist :" << m_DiffBetweenComAndWaist,"DebugData.txt");
// the initial position does not put z at Zc
// The most important line of the method...
......@@ -685,7 +685,7 @@ KinematicsForOneLeg
// Compute the inverse kinematics.
if ((LeftOrRight==1) && (Stage==0))
{
ODEBUG5SIMPLE(Body_P[0] << " " <<
ODEBUG4SIMPLE(Body_P[0] << " " <<
Body_P[1] << " " <<
Body_P[2] << " " <<
Foot_P[0] << " " <<
......@@ -809,11 +809,11 @@ KinematicsForTheLegs
m_FinalDesiredCOMPose(2,3) = aCoMPosition(2);
m_FinalDesiredCOMPose(3,3) = 1.0;
ODEBUG5(Body_P ,"DebugDataBodyP1.dat");
ODEBUG4(Body_P ,"DebugDataBodyP1.dat");
}
else
{
ODEBUG5(Body_P ,"DebugDataBodyP0.dat");
ODEBUG4(Body_P ,"DebugDataBodyP0.dat");
}
......@@ -841,7 +841,7 @@ KinematicsForTheLegs
aCoMPosition,
ToTheHip,-1,qr,Stage);
ODEBUG5("**************","DebugDataIK.dat");
ODEBUG4("**************","DebugDataIK.dat");
/* Should compute now the Waist Position */
m_ComAndWaistInRefFrame=Body_R*m_DiffBetweenComAndWaist;
......@@ -1011,7 +1011,7 @@ ComputePostureForGivenCoMAndFeetPosture
}
}
ODEBUG5(CurrentVelocity, "DebugDataVelocity0.dat");
ODEBUG4(CurrentVelocity, "DebugDataVelocity0.dat");
m_prev_Configuration = CurrentConfiguration;
m_prev_Velocity = CurrentVelocity;
}
......@@ -1037,7 +1037,7 @@ ComputePostureForGivenCoMAndFeetPosture
{
CurrentVelocity.setZero();
}
ODEBUG5(CurrentVelocity, "DebugDataVelocity1.dat");
ODEBUG4(CurrentVelocity, "DebugDataVelocity1.dat");
m_prev_Configuration1 = CurrentConfiguration;
m_prev_Velocity1 = CurrentVelocity;
}
......@@ -1066,7 +1066,7 @@ ComputePostureForGivenCoMAndFeetPosture
{
CurrentVelocity.setZero();
}
ODEBUG5(CurrentVelocity, "DebugDataVelocity1.dat");
ODEBUG4(CurrentVelocity, "DebugDataVelocity1.dat");
m_prev_Configuration2 = CurrentConfiguration;
m_prev_Velocity2 = CurrentVelocity;
}
......@@ -1122,20 +1122,20 @@ ComputePostureForGivenCoMAndFeetPosture
ODEBUG( "CurrentVelocity :" << endl << CurrentVelocity);
ODEBUG5("SamplingPeriod " << getSamplingPeriod(),"LegsSpeed.dat");
ODEBUG4("SamplingPeriod " << getSamplingPeriod(),"LegsSpeed.dat");
string aDebugFileName;
ODEBUG5( (1.0/M_PI)*180.0*lql[0] << " " <<
ODEBUG4( (1.0/M_PI)*180.0*lql[0] << " " <<
(1.0/M_PI)*180.0*lql[1] << " " <<
(1.0/M_PI)*180.0*lql[2] << " " <<
(1.0/M_PI)*180.0*lql[3] << " " <<
(1.0/M_PI)*180.0*lql[4] << " " <<
(1.0/M_PI)*180.0*lql[5],(char *)aDebugFileName.c_str());
ODEBUG5(CurrentVelocity,"DebugDataVelocity.dat");
ODEBUG4(CurrentVelocity,"DebugDataVelocity.dat");
ODEBUG5( aCoMPosition[0]
ODEBUG4( aCoMPosition[0]
<< " " <<
aCoMPosition[1],"COMPC1.dat");
......@@ -1217,11 +1217,11 @@ void ComAndFootRealizationByGeometry::
Eigen::VectorXd & LFP)
{
ODEBUG5("aCOMPosition:" << aCOMPosition << endl <<
ODEBUG4("aCOMPosition:" << aCOMPosition << endl <<
"Right Foot Position:" << RFP << endl <<
"Left Foot Position:" << LFP << endl, "DebugDataIKArms.txt");
ODEBUG5(m_ZARM << " " << m_Xmax << " " << " " << m_GainFactor,"DebugDataIKArms.txt");
ODEBUG4(m_ZARM << " " << m_Xmax << " " << " " << m_GainFactor,"DebugDataIKArms.txt");
double TempXL,TempXR,TempCos,TempSin,
TempARight,TempALeft;
......@@ -1235,14 +1235,14 @@ void ComAndFootRealizationByGeometry::
TempXL = TempCos * (LFP(0) +m_AnklePositionRight[0] - aCOMPosition(0) - m_COGInitialAnkles(0)) +
TempSin * (LFP(1) +m_AnklePositionRight[1] - aCOMPosition(1) - m_COGInitialAnkles(1));
ODEBUG5(aCOMPosition(0) << " " << aCOMPosition(1) << " " << aCOMPosition(3),"DebugDataIKArms.txt");
ODEBUG5(RFP(0) << " " << RFP(1) ,"DebugDataIKArms.txt");
ODEBUG5(LFP(0) << " " << LFP(1) ,"DebugDataIKArms.txt");
ODEBUG4(aCOMPosition(0) << " " << aCOMPosition(1) << " " << aCOMPosition(3),"DebugDataIKArms.txt");
ODEBUG4(RFP(0) << " " << RFP(1) ,"DebugDataIKArms.txt");
ODEBUG4(LFP(0) << " " << LFP(1) ,"DebugDataIKArms.txt");
TempARight = TempXR*-1.0;
TempALeft = TempXL*-1.0;
ODEBUG5("Values: TL " << TempALeft <<
ODEBUG4("Values: TL " << TempALeft <<
" TR " << TempARight <<
" " << m_ZARM <<
" " << m_Xmax ,"DebugDataIKArms.txt");
......@@ -1266,8 +1266,8 @@ void ComAndFootRealizationByGeometry::
jointRootPosition,
jointEndPosition,
qArml);
ODEBUG5("ComputeHeuristicArm: Step 2 ","DebugDataIKArms.txt");
ODEBUG5( "IK Left arm p:" << qArml(0)<< " " << qArml(1) << " " << qArml(2)
ODEBUG4("ComputeHeuristicArm: Step 2 ","DebugDataIKArms.txt");
ODEBUG4( "IK Left arm p:" << qArml(0)<< " " << qArml(1) << " " << qArml(2)
<< " " << qArml(3) << " " << qArml(4) << " " << qArml(5), "DebugDataIKArms.txt" );
jointEndPosition(0,3) = TempARight;
......@@ -1279,10 +1279,10 @@ void ComAndFootRealizationByGeometry::
jointRootPosition,
jointEndPosition,
qArmr);
ODEBUG5( "IK Right arm p:" << qArmr(0)<< " " << qArmr(1) << " " << qArmr(2)
ODEBUG4( "IK Right arm p:" << qArmr(0)<< " " << qArmr(1) << " " << qArmr(2)
<< " " << qArmr(3) << " " << qArmr(4) << " " << qArmr(5), "DebugDataIKArms.txt" );
ODEBUG5( qArml(0)<< " " << qArml(1) << " " << qArml(2) << " "
ODEBUG4( qArml(0)<< " " << qArml(1) << " " << qArml(2) << " "
<< qArml(3) << " " << qArml(4) << " " << qArml(5) << " "
<< qArmr(0)<< " " << qArmr(1) << " " << qArmr(2) << " "
<< qArmr(3) << " " << qArmr(4) << " " << qArmr(5),
......
......@@ -70,9 +70,9 @@ ZMPPreviewControlWithMultiBodyZMP(SimplePluginManager *lSPM)
RESETDEBUG4("DebugConfSO.dat");
RESETDEBUG4("DebugConfSV.dat");
RESETDEBUG4("DebugConfSA.dat");
RESETDEBUG5("DebugDataCheckZMP1.txt");
RESETDEBUG4("DebugDataCheckZMP1.txt");
RESETDEBUG4("2ndStage.dat");
RESETDEBUG5("ZMPPCWMZOGSOC.dat");
RESETDEBUG4("ZMPPCWMZOGSOC.dat");
// Sampling period.DMB
m_SamplingPeriod = -1;
......
......@@ -420,11 +420,11 @@ void PinocchioRobot::initializeLegsInverseKinematics()
{
m_isLegInverseKinematic=false;
}
RESETDEBUG5("DebugDataInitIK.dat");
ODEBUG5("waist_M_leftHip " << waist_M_leftHip,"DebugDataInitIK.dat");
ODEBUG5("waist_M_rightHip " << waist_M_rightHip,"DebugDataInitIK.dat");
ODEBUG5("m_leftDt " << m_leftDt,"DebugDataInitIK.dat");
ODEBUG5("m_rightDt " << m_rightDt,"DebugDataInitIK.dat");
RESETDEBUG4("DebugDataInitIK.dat");
ODEBUG4("waist_M_leftHip " << waist_M_leftHip,"DebugDataInitIK.dat");
ODEBUG4("waist_M_rightHip " << waist_M_rightHip,"DebugDataInitIK.dat");
ODEBUG4("m_leftDt " << m_leftDt,"DebugDataInitIK.dat");
ODEBUG4("m_rightDt " << m_rightDt,"DebugDataInitIK.dat");
return ;
}
......
......@@ -200,15 +200,18 @@ Solution_(),OFTG_DF_(0),OFTG_control_(0),dynamicFilter_(0)
":numberstepsbeforestop",
":stoppg",
":setfeetconstraint"};
RESETDEBUG5("PgDebug2.txt");
ODEBUG5("Before registering methods for ZMPVelocityReferencedQP","PgDebug2.txt");
RESETDEBUG4("PgDebug2.txt");
ODEBUG4("Before registering methods for ZMPVelocityReferencedQP",
"PgDebug2.txt");
for(unsigned int i=0;i<NbMethods;i++)
{
//#ifdef DEBUG
// std::cout << "lMethodNames["<< i << "]=" << lMethodNames[i] <<std::endl;
//#endif
//#ifdef DEBUG
// std::cout << "lMethodNames["<< i << "]="
// << lMethodNames[i] <<std::endl;
//#endif
std::string aMethodName(lMethodNames[i]);
ODEBUG5("Register method " << aMethodName << "for ZMPVelocityReferencedQP","PgDebug2.txt");
ODEBUG4("Register method " << aMethodName
<< "for ZMPVelocityReferencedQP","PgDebug2.txt");
if (!RegisterMethod(aMethodName))
{
std::cerr << "Unable to register " << aMethodName << std::endl;
......
......@@ -25,6 +25,9 @@
/*! \file nmpc_generator.cpp
\brief implement an SQP method to generate online stable walking motion */
#define EIGEN_RUNTIME_NO_MALLOC
#include <Eigen/Dense>
#include <ZMPRefTrajectoryGeneration/nmpc_generator.hh>
#include <cmath>
#include <Debug.hh>
......@@ -33,6 +36,8 @@
//#define DEBUG_COUT
//#ifdef DEBUG
void DumpMatrix(std::string fileName, Eigen::MatrixXd & M)
{
std::ofstream aof;
......@@ -755,6 +760,7 @@ void NMPCgenerator::solve()
{
if(currentSupport_.Phase==DS && currentSupport_.NbStepsLeft == 0)
return;
Eigen::internal::set_is_malloc_allowed(false);
/* Process and solve problem, s.t. pattern generator data is consistent */
unsigned iter = 0 ;
oneMoreStep_ = true;
......@@ -785,6 +791,7 @@ void NMPCgenerator::solve()
os.open("iteration_solver.dat",ios::out);
++iteration_solver_file ;
}
Eigen::internal::set_is_malloc_allowed(true);
ofstream os("iteration_solver.dat",ios::app);
os << time_ << " "
<< iter-1 << " "
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment