From 2aa46fca537209f499f797833594542704622aef Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Thu, 22 Sep 2016 08:03:12 +0200
Subject: [PATCH] Fix new buildModel interface and indentation.
---
tests/TestObject.cpp | 804 ++++++++++++++++++++++---------------------
1 file changed, 403 insertions(+), 401 deletions(-)
diff --git a/tests/TestObject.cpp b/tests/TestObject.cpp
index 66df47c9..dcfe4fff 100644
--- a/tests/TestObject.cpp
+++ b/tests/TestObject.cpp
@@ -127,12 +127,12 @@ namespace PatternGeneratorJRL
try{
checkFiles();
}catch(std::string e)
- {
- std::cout << "file problem, existance or extension incorrect"
- << std::endl ;
- std::cout << e << std::endl ;
- return false;
- }
+ {
+ std::cout << "file problem, existance or extension incorrect"
+ << std::endl ;
+ std::cout << e << std::endl ;
+ return false;
+ }
// Instanciate and initialize.
CreateAndInitializeHumanoidRobot(m_URDFPath,m_SRDFPath,m_PR,m_DebugPR);
@@ -155,18 +155,18 @@ namespace PatternGeneratorJRL
MAL_VECTOR_RESIZE(m_PreviousVelocity ,lNbDofs);
MAL_VECTOR_RESIZE(m_PreviousAcceleration ,lNbDofs);
for(int i=0;i<6;i++)
- {
- m_PreviousConfiguration[i] = 0.0 ;
- m_PreviousVelocity[i] = 0.0 ;
- m_PreviousAcceleration[i] = 0.0;
- }
+ {
+ m_PreviousConfiguration[i] = 0.0 ;
+ m_PreviousVelocity[i] = 0.0 ;
+ m_PreviousAcceleration[i] = 0.0;
+ }
for(unsigned int i=6;i<lNbDofs;i++)
- {
- m_PreviousConfiguration[i] = m_HalfSitting[i-6];
- m_PreviousVelocity[i] = 0.0 ;
- m_PreviousAcceleration[i] = 0.0;
- }
+ {
+ m_PreviousConfiguration[i] = m_HalfSitting[i-6];
+ m_PreviousVelocity[i] = 0.0 ;
+ m_PreviousAcceleration[i] = 0.0;
+ }
return true ;
}
@@ -190,31 +190,33 @@ namespace PatternGeneratorJRL
}
void TestObject::CreateAndInitializeHumanoidRobot(
- std::string &URDFFile,
- std::string &SRDFFile,
- PinocchioRobot *& aPR,
- PinocchioRobot *& aDebugPR)
+ std::string &URDFFile,
+ std::string &SRDFFile,
+ PinocchioRobot *& aPR,
+ PinocchioRobot *& aDebugPR)
{
// Creating the humanoid robot via the URDF.
-// try{
- m_robotModel = se3::urdf::buildModel(URDFFile, se3::JointModelFreeFlyer());
- m_robotData = new se3::Data(m_robotModel) ;
- m_DebugRobotData = new se3::Data(m_robotModel) ;
-// }catch(std::invalid_argument e)
-// {
-// cout << e.what() ;
-// cout << "robot model or robot data not created properly" << endl ;
-// return ;
-// }
+ // try{
+ se3::urdf::buildModel(URDFFile,
+ se3::JointModelFreeFlyer(),
+ m_robotModel);
+ m_robotData = new se3::Data(m_robotModel) ;
+ m_DebugRobotData = new se3::Data(m_robotModel) ;
+ // }catch(std::invalid_argument e)
+ // {
+ // cout << e.what() ;
+ // cout << "robot model or robot data not created properly" << endl ;
+ // return ;
+ // }
if ((aPR==0) || (aDebugPR==0))
- {
- if (aPR!=0) delete aPR;
- if (aDebugPR!=0) delete aDebugPR;
+ {
+ if (aPR!=0) delete aPR;
+ if (aDebugPR!=0) delete aDebugPR;
- aPR = new PinocchioRobot();
- aDebugPR = new PinocchioRobot();
- }
+ aPR = new PinocchioRobot();
+ aDebugPR = new PinocchioRobot();
+ }
// initialize the model and data of the humanoid robot
aPR->initializeRobotModelAndData(&m_robotModel,m_robotData);
@@ -233,58 +235,58 @@ namespace PatternGeneratorJRL
//////////////////////////////////
std::ifstream srdf_stream(filename.c_str());
if (! srdf_stream.is_open())
- {
- const std::string exception_message (filename + " does not seem to be a valid file.");
- cerr << exception_message << endl ;
- throw std::invalid_argument(exception_message);
- }
+ {
+ const std::string exception_message (filename + " does not seem to be a valid file.");
+ cerr << exception_message << endl ;
+ throw std::invalid_argument(exception_message);
+ }
// Read xml stream
using boost::property_tree::ptree;
ptree pt;
try{
read_xml(srdf_stream, pt);
}catch(...)
- {
- cerr << "problem while reading the srdf file. File corrupted?" << endl;
- return ;
- }
+ {
+ cerr << "problem while reading the srdf file. File corrupted?" << endl;
+ return ;
+ }
// Get the starting configuration : half sitting
MAL_VECTOR_RESIZE(m_HalfSitting,aPR.numberDof()-6);
MAL_VECTOR_FILL(m_HalfSitting,0.0);
se3::Model * aModel = aPR.Model();
BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot.group_state"))
- {
- if(v.first=="joint")
- {
- const std::string jointName =
- v.second.get<std::string>("<xmlattr>.name");
- const double jointValue =
- v.second.get<double>("<xmlattr>.value");
- if(aModel->existJointName(jointName))
- {
- se3::JointIndex id = aModel->getJointId(jointName);
- unsigned idq = se3::idx_q(aModel->joints[id]);
- // we assume only revolute joint here.
- m_HalfSitting(idq-7) = jointValue ;
- }
- }
- } // BOOST_FOREACH
+ {
+ if(v.first=="joint")
+ {
+ const std::string jointName =
+ v.second.get<std::string>("<xmlattr>.name");
+ const double jointValue =
+ v.second.get<double>("<xmlattr>.value");
+ if(aModel->existJointName(jointName))
+ {
+ se3::JointIndex id = aModel->getJointId(jointName);
+ unsigned idq = se3::idx_q(aModel->joints[id]);
+ // we assume only revolute joint here.
+ m_HalfSitting(idq-7) = jointValue ;
+ }
+ }
+ } // BOOST_FOREACH
bool DebugConfiguration = true;
if (DebugConfiguration)
- {
- ofstream aofq;
- aofq.open("TestConfiguration.dat",ofstream::out);
- if (aofq.is_open())
- {
- for(unsigned int k=0;k<MAL_VECTOR_SIZE(m_HalfSitting);k++)
- {
- aofq << m_HalfSitting(k) << " ";
- }
- aofq << endl;
- }
-
- }
+ {
+ ofstream aofq;
+ aofq.open("TestConfiguration.dat",ofstream::out);
+ if (aofq.is_open())
+ {
+ for(unsigned int k=0;k<MAL_VECTOR_SIZE(m_HalfSitting);k++)
+ {
+ aofq << m_HalfSitting(k) << " ";
+ }
+ aofq << endl;
+ }
+
+ }
// capture the details of the feet
//////////////////////////////////
@@ -293,18 +295,18 @@ namespace PatternGeneratorJRL
PRFoot aFoot ;
string path = "robot.specificities.feet.right.size" ;
BOOST_FOREACH(const ptree::value_type & v, pt.get_child(path.c_str()))
- {
- aFoot.soleHeight = v.second.get<double>("height");
- aFoot.soleWidth = v.second.get<double>("width");
- aFoot.soleDepth = v.second.get<double>("depth");
- }
+ {
+ aFoot.soleHeight = v.second.get<double>("height");
+ aFoot.soleWidth = v.second.get<double>("width");
+ aFoot.soleDepth = v.second.get<double>("depth");
+ }
path = "robot.specificities.feet.right.anklePosition" ;
BOOST_FOREACH(const ptree::value_type & v, pt.get_child(path.c_str()))
- {
- aFoot.anklePosition(0) = v.second.get<double>("x");
- aFoot.anklePosition(1) = v.second.get<double>("y");
- aFoot.anklePosition(2) = v.second.get<double>("z");
- }
+ {
+ aFoot.anklePosition(0) = v.second.get<double>("x");
+ aFoot.anklePosition(1) = v.second.get<double>("y");
+ aFoot.anklePosition(2) = v.second.get<double>("z");
+ }
se3::FrameIndex ra = aModel->getFrameId("r_ankle");
aFoot.associatedAnkle = aModel->frames[ra].parent ;
aPR.initializeRightFoot(aFoot);
@@ -312,18 +314,18 @@ namespace PatternGeneratorJRL
// Initialize the Left Foot
path = "robot.specificities.feet.left.size" ;
BOOST_FOREACH(const ptree::value_type & v, pt.get_child(path.c_str()))
- {
- aFoot.soleHeight = v.second.get<double>("height");
- aFoot.soleWidth = v.second.get<double>("width");
- aFoot.soleDepth = v.second.get<double>("depth");
- }
+ {
+ aFoot.soleHeight = v.second.get<double>("height");
+ aFoot.soleWidth = v.second.get<double>("width");
+ aFoot.soleDepth = v.second.get<double>("depth");
+ }
path = "robot.specificities.feet.left.anklePosition" ;
BOOST_FOREACH(const ptree::value_type & v, pt.get_child(path.c_str()))
- {
- aFoot.anklePosition(0) = v.second.get<double>("x");
- aFoot.anklePosition(1) = v.second.get<double>("y");
- aFoot.anklePosition(2) = v.second.get<double>("z");
- }
+ {
+ aFoot.anklePosition(0) = v.second.get<double>("x");
+ aFoot.anklePosition(1) = v.second.get<double>("y");
+ aFoot.anklePosition(2) = v.second.get<double>("z");
+ }
se3::FrameIndex la = aModel->getBodyId("l_ankle");
aFoot.associatedAnkle = aModel->frames[la].parent ;
aPR.initializeLeftFoot(aFoot);
@@ -333,138 +335,138 @@ namespace PatternGeneratorJRL
{
if (m_DebugZMP2)
- {
- ofstream aofzmpmb2;
- string aFileName = m_TestName;
- aFileName += "ZMPMBSTAGE2.dat";
- aofzmpmb2.open(aFileName.c_str(),ofstream::out);
- }
+ {
+ ofstream aofzmpmb2;
+ string aFileName = m_TestName;
+ aFileName += "ZMPMBSTAGE2.dat";
+ aofzmpmb2.open(aFileName.c_str(),ofstream::out);
+ }
if (m_DebugFGPI)
- {
- ofstream aof;
- string aFileName = m_TestName;
- aFileName += "TestFGPI_description.dat";
-
- aof.open(aFileName.c_str(),ofstream::out);
-
- string Titles[NB_OF_FIELDS] =
- { "Time",
- "Com X",
- "Com Y" ,
- "Com Z" ,
- "Com Yaw",
- "Com dX" ,
- "Com dY" ,
- "Com dZ" ,
- "ZMP X (world ref.)" ,
- "ZMP Y (world ref.)" ,
- "Left Foot X" ,
- "Left Foot Y" ,
- "Left Foot Z" ,
- "Left Foot dX" ,
- "Left Foot dY" ,
- "Left Foot dZ" ,
- "Left Foot ddX" ,
- "Left Foot ddY" ,
- "Left Foot ddZ" ,
- "Left Foot Theta" ,
- "Left Foot Omega" ,
- "Left Foot Omega2" ,
- "Right Foot X" ,
- "Right Foot Y" ,
- "Right Foot Z" ,
- "Right Foot dX" ,
- "Right Foot dY" ,
- "Right Foot dZ" ,
- "Right Foot ddX" ,
- "Right Foot ddY" ,
- "Right Foot ddZ" ,
- "Right Foot Theta" ,
- "Right Foot Omega" ,
- "Right Foot Omega2" ,
- "ZMP X (waist ref.)" ,
- "ZMP Y (waist ref.)" ,
- "Waist X (world ref.)" ,
- "Waist Y (world ref.)" ,
- "all configuration vector"};
- for(unsigned int i=0;i<NB_OF_FIELDS;i++)
- aof << i+1 << ". " <<Titles[i] <<std::endl;
-
- aof.close();
-
- aFileName = m_TestName;
- aFileName += "TestFGPI.dat";
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
+ {
+ ofstream aof;
+ string aFileName = m_TestName;
+ aFileName += "TestFGPI_description.dat";
+
+ aof.open(aFileName.c_str(),ofstream::out);
+
+ string Titles[NB_OF_FIELDS] =
+ { "Time",
+ "Com X",
+ "Com Y" ,
+ "Com Z" ,
+ "Com Yaw",
+ "Com dX" ,
+ "Com dY" ,
+ "Com dZ" ,
+ "ZMP X (world ref.)" ,
+ "ZMP Y (world ref.)" ,
+ "Left Foot X" ,
+ "Left Foot Y" ,
+ "Left Foot Z" ,
+ "Left Foot dX" ,
+ "Left Foot dY" ,
+ "Left Foot dZ" ,
+ "Left Foot ddX" ,
+ "Left Foot ddY" ,
+ "Left Foot ddZ" ,
+ "Left Foot Theta" ,
+ "Left Foot Omega" ,
+ "Left Foot Omega2" ,
+ "Right Foot X" ,
+ "Right Foot Y" ,
+ "Right Foot Z" ,
+ "Right Foot dX" ,
+ "Right Foot dY" ,
+ "Right Foot dZ" ,
+ "Right Foot ddX" ,
+ "Right Foot ddY" ,
+ "Right Foot ddZ" ,
+ "Right Foot Theta" ,
+ "Right Foot Omega" ,
+ "Right Foot Omega2" ,
+ "ZMP X (waist ref.)" ,
+ "ZMP Y (waist ref.)" ,
+ "Waist X (world ref.)" ,
+ "Waist Y (world ref.)" ,
+ "all configuration vector"};
+ for(unsigned int i=0;i<NB_OF_FIELDS;i++)
+ aof << i+1 << ". " <<Titles[i] <<std::endl;
+
+ aof.close();
+
+ aFileName = m_TestName;
+ aFileName += "TestFGPI.dat";
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
}
void TestObject::fillInDebugFiles( )
{
if (m_DebugFGPI)
- {
- double localZMPx = m_OneStep.ZMPTarget(0)*cos(m_CurrentConfiguration(5)) -
+ {
+ double localZMPx = m_OneStep.ZMPTarget(0)*cos(m_CurrentConfiguration(5)) -
m_OneStep.ZMPTarget(1)*sin(m_CurrentConfiguration(5)) +
m_CurrentConfiguration(0) ;
- double localZMPy = m_OneStep.ZMPTarget(0)*sin(m_CurrentConfiguration(5)) +
+ double localZMPy = m_OneStep.ZMPTarget(0)*sin(m_CurrentConfiguration(5)) +
m_OneStep.ZMPTarget(1)*cos(m_CurrentConfiguration(5)) +
m_CurrentConfiguration(1) ;
- ofstream aof;
- string aFileName;
- aFileName = m_TestName;
- aFileName += "TestFGPI.dat";
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision(m_OneStep.NbOfIt*0.005 ) << " " // 1
- << filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 2
- << filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
- << filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " " // 4
- << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " " // 5
- << filterprecision(m_OneStep.finalCOMPosition.x[1] ) << " " // 6
- << filterprecision(m_OneStep.finalCOMPosition.y[1] ) << " " // 7
- << filterprecision(m_OneStep.finalCOMPosition.z[1] ) << " " // 8
- << filterprecision(m_OneStep.ZMPTarget(0) ) << " " // 9
- << filterprecision(m_OneStep.ZMPTarget(1) ) << " " // 10
- << filterprecision(m_OneStep.LeftFootPosition.x ) << " " // 11
- << filterprecision(m_OneStep.LeftFootPosition.y ) << " " // 12
- << filterprecision(m_OneStep.LeftFootPosition.z ) << " " // 13
- << filterprecision(m_OneStep.LeftFootPosition.dx ) << " " // 14
- << filterprecision(m_OneStep.LeftFootPosition.dy ) << " " // 15
- << filterprecision(m_OneStep.LeftFootPosition.dz ) << " " // 16
- << filterprecision(m_OneStep.LeftFootPosition.ddx ) << " " // 17
- << filterprecision(m_OneStep.LeftFootPosition.ddy ) << " " // 18
- << filterprecision(m_OneStep.LeftFootPosition.ddz ) << " " // 19
- << filterprecision(m_OneStep.LeftFootPosition.theta*M_PI/180 ) << " " // 20
- << filterprecision(m_OneStep.LeftFootPosition.omega*M_PI/180 ) << " " // 21
- << filterprecision(m_OneStep.LeftFootPosition.omega2*M_PI/180 ) << " " // 22
- << filterprecision(m_OneStep.RightFootPosition.x ) << " " // 23
- << filterprecision(m_OneStep.RightFootPosition.y ) << " " // 24
- << filterprecision(m_OneStep.RightFootPosition.z ) << " " // 25
- << filterprecision(m_OneStep.RightFootPosition.dx ) << " " // 26
- << filterprecision(m_OneStep.RightFootPosition.dy ) << " " // 27
- << filterprecision(m_OneStep.RightFootPosition.dz ) << " " // 28
- << filterprecision(m_OneStep.RightFootPosition.ddx ) << " " // 29
- << filterprecision(m_OneStep.RightFootPosition.ddy ) << " " // 30
- << filterprecision(m_OneStep.RightFootPosition.ddz ) << " " // 31
- << filterprecision(m_OneStep.RightFootPosition.theta*M_PI/180 ) << " " // 32
- << filterprecision(m_OneStep.RightFootPosition.omega*M_PI/180 ) << " " // 33
- << filterprecision(m_OneStep.RightFootPosition.omega2*M_PI/180 ) << " " // 34
- << filterprecision(localZMPx) << " " // 35
- << filterprecision(localZMPy) << " " // 36
- << filterprecision(m_CurrentConfiguration(0) ) << " " // 37
- << filterprecision(m_CurrentConfiguration(1) ) << " " ;// 38
- for (unsigned int i = 0 ; i < m_PR->currentConfiguration().size() ; i++)
- {
- aof << filterprecision(m_PR->currentConfiguration()(i)) << " " ; // 39 - 39+dofs
- }
- aof << endl;
- aof.close();
- }
+ ofstream aof;
+ string aFileName;
+ aFileName = m_TestName;
+ aFileName += "TestFGPI.dat";
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision(m_OneStep.NbOfIt*0.005 ) << " " // 1
+ << filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 2
+ << filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
+ << filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " " // 4
+ << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " " // 5
+ << filterprecision(m_OneStep.finalCOMPosition.x[1] ) << " " // 6
+ << filterprecision(m_OneStep.finalCOMPosition.y[1] ) << " " // 7
+ << filterprecision(m_OneStep.finalCOMPosition.z[1] ) << " " // 8
+ << filterprecision(m_OneStep.ZMPTarget(0) ) << " " // 9
+ << filterprecision(m_OneStep.ZMPTarget(1) ) << " " // 10
+ << filterprecision(m_OneStep.LeftFootPosition.x ) << " " // 11
+ << filterprecision(m_OneStep.LeftFootPosition.y ) << " " // 12
+ << filterprecision(m_OneStep.LeftFootPosition.z ) << " " // 13
+ << filterprecision(m_OneStep.LeftFootPosition.dx ) << " " // 14
+ << filterprecision(m_OneStep.LeftFootPosition.dy ) << " " // 15
+ << filterprecision(m_OneStep.LeftFootPosition.dz ) << " " // 16
+ << filterprecision(m_OneStep.LeftFootPosition.ddx ) << " " // 17
+ << filterprecision(m_OneStep.LeftFootPosition.ddy ) << " " // 18
+ << filterprecision(m_OneStep.LeftFootPosition.ddz ) << " " // 19
+ << filterprecision(m_OneStep.LeftFootPosition.theta*M_PI/180 ) << " " // 20
+ << filterprecision(m_OneStep.LeftFootPosition.omega*M_PI/180 ) << " " // 21
+ << filterprecision(m_OneStep.LeftFootPosition.omega2*M_PI/180 ) << " " // 22
+ << filterprecision(m_OneStep.RightFootPosition.x ) << " " // 23
+ << filterprecision(m_OneStep.RightFootPosition.y ) << " " // 24
+ << filterprecision(m_OneStep.RightFootPosition.z ) << " " // 25
+ << filterprecision(m_OneStep.RightFootPosition.dx ) << " " // 26
+ << filterprecision(m_OneStep.RightFootPosition.dy ) << " " // 27
+ << filterprecision(m_OneStep.RightFootPosition.dz ) << " " // 28
+ << filterprecision(m_OneStep.RightFootPosition.ddx ) << " " // 29
+ << filterprecision(m_OneStep.RightFootPosition.ddy ) << " " // 30
+ << filterprecision(m_OneStep.RightFootPosition.ddz ) << " " // 31
+ << filterprecision(m_OneStep.RightFootPosition.theta*M_PI/180 ) << " " // 32
+ << filterprecision(m_OneStep.RightFootPosition.omega*M_PI/180 ) << " " // 33
+ << filterprecision(m_OneStep.RightFootPosition.omega2*M_PI/180 ) << " " // 34
+ << filterprecision(localZMPx) << " " // 35
+ << filterprecision(localZMPy) << " " // 36
+ << filterprecision(m_CurrentConfiguration(0) ) << " " // 37
+ << filterprecision(m_CurrentConfiguration(1) ) << " " ;// 38
+ for (unsigned int i = 0 ; i < m_PR->currentConfiguration().size() ; i++)
+ {
+ aof << filterprecision(m_PR->currentConfiguration()(i)) << " " ; // 39 - 39+dofs
+ }
+ aof << endl;
+ aof.close();
+ }
}
@@ -474,122 +476,122 @@ namespace PatternGeneratorJRL
{
bool SameFile= false;
if (m_DebugFGPI)
- {
- SameFile = true;
- ifstream alif;
- string aFileName;
- aFileName = m_TestName;
- aFileName += "TestFGPI.dat";
- ODEBUG("Report:" << aFileName);
- unsigned max_nb_of_pbs=100;
- unsigned nb_of_pbs = 0;
-
- alif.open(aFileName.c_str(),ifstream::in);
- if (!alif.is_open())
- {
- std::cerr << "Unable to open "<< aFileName << std::endl;
- return false;
- }
-
- ifstream arif;
- aFileName = m_TestName;
- aFileName += "TestFGPI.datref";
- arif.open(aFileName.c_str(),ifstream::in);
- ODEBUG("ReportRef:" << aFileName);
-
- if (!arif.is_open())
- {
- std::cerr << "Unable to open "<< aFileName << std::endl;
- return false;
- }
-
-
- ofstream areportof;
- aFileName = m_TestName;
- aFileName += "TestFGPI_report.dat";
- areportof.open(aFileName.c_str(),ofstream::out);
-
- // Time
- double LocalInput[NB_OF_FIELDS], ReferenceInput[NB_OF_FIELDS];
- bool finalreport = true;
- unsigned long int nblines = 0;
- bool endofinspection=false;
-
- // Find size of the two files.
- alif.seekg (0, alif.end);
- unsigned long int alif_length = (unsigned long int)alif.tellg();
- alif.seekg (0, alif.beg);
-
- arif.seekg (0, arif.end);
- unsigned long int arif_length = (unsigned long int)arif.tellg();
- arif.seekg (0, arif.beg);
-
- while ((!alif.eof()) &&
- (!arif.eof()) &&
- (!endofinspection))
- {
- for (unsigned int i=0;i<NB_OF_FIELDS;i++)
- {
- alif >> LocalInput[i];
- if (alif.eof())
- {
- endofinspection =true;
- break;
- }
- }
- if (endofinspection)
- break;
-
- for (unsigned int i=0;i<NB_OF_FIELDS;i++)
- {
- arif >> ReferenceInput[i];
- if (arif.eof())
- {
- endofinspection =true;
- break;
- }
- }
- if (endofinspection)
- break;
-
-
- for (unsigned int i=0;i<NB_OF_FIELDS;i++)
- {
- if (fabs(LocalInput[i]-
- ReferenceInput[i])>=1e-6)
- {
- finalreport = false;
- ostringstream oss;
- oss << "l: " << nblines
- << " col:" << i
- << " ref: " << ReferenceInput[i]
- << " now: " << LocalInput[i]
- << " " << nb_of_pbs
- <<std::endl;
- areportof << oss.str();
- std::cout << oss.str();
- nb_of_pbs++;
- if(nb_of_pbs>max_nb_of_pbs)
- {
- endofinspection=true;
- }
- }
- }
-
- nblines++;
- if ((nblines*2> alif_length) ||
- (nblines*2> arif_length))
- {
- endofinspection=true;
- break;
- }
- }
-
- alif.close();
- arif.close();
- areportof.close();
- return finalreport;
- }
+ {
+ SameFile = true;
+ ifstream alif;
+ string aFileName;
+ aFileName = m_TestName;
+ aFileName += "TestFGPI.dat";
+ ODEBUG("Report:" << aFileName);
+ unsigned max_nb_of_pbs=100;
+ unsigned nb_of_pbs = 0;
+
+ alif.open(aFileName.c_str(),ifstream::in);
+ if (!alif.is_open())
+ {
+ std::cerr << "Unable to open "<< aFileName << std::endl;
+ return false;
+ }
+
+ ifstream arif;
+ aFileName = m_TestName;
+ aFileName += "TestFGPI.datref";
+ arif.open(aFileName.c_str(),ifstream::in);
+ ODEBUG("ReportRef:" << aFileName);
+
+ if (!arif.is_open())
+ {
+ std::cerr << "Unable to open "<< aFileName << std::endl;
+ return false;
+ }
+
+
+ ofstream areportof;
+ aFileName = m_TestName;
+ aFileName += "TestFGPI_report.dat";
+ areportof.open(aFileName.c_str(),ofstream::out);
+
+ // Time
+ double LocalInput[NB_OF_FIELDS], ReferenceInput[NB_OF_FIELDS];
+ bool finalreport = true;
+ unsigned long int nblines = 0;
+ bool endofinspection=false;
+
+ // Find size of the two files.
+ alif.seekg (0, alif.end);
+ unsigned long int alif_length = (unsigned long int)alif.tellg();
+ alif.seekg (0, alif.beg);
+
+ arif.seekg (0, arif.end);
+ unsigned long int arif_length = (unsigned long int)arif.tellg();
+ arif.seekg (0, arif.beg);
+
+ while ((!alif.eof()) &&
+ (!arif.eof()) &&
+ (!endofinspection))
+ {
+ for (unsigned int i=0;i<NB_OF_FIELDS;i++)
+ {
+ alif >> LocalInput[i];
+ if (alif.eof())
+ {
+ endofinspection =true;
+ break;
+ }
+ }
+ if (endofinspection)
+ break;
+
+ for (unsigned int i=0;i<NB_OF_FIELDS;i++)
+ {
+ arif >> ReferenceInput[i];
+ if (arif.eof())
+ {
+ endofinspection =true;
+ break;
+ }
+ }
+ if (endofinspection)
+ break;
+
+
+ for (unsigned int i=0;i<NB_OF_FIELDS;i++)
+ {
+ if (fabs(LocalInput[i]-
+ ReferenceInput[i])>=1e-6)
+ {
+ finalreport = false;
+ ostringstream oss;
+ oss << "l: " << nblines
+ << " col:" << i
+ << " ref: " << ReferenceInput[i]
+ << " now: " << LocalInput[i]
+ << " " << nb_of_pbs
+ <<std::endl;
+ areportof << oss.str();
+ std::cout << oss.str();
+ nb_of_pbs++;
+ if(nb_of_pbs>max_nb_of_pbs)
+ {
+ endofinspection=true;
+ }
+ }
+ }
+
+ nblines++;
+ if ((nblines*2> alif_length) ||
+ (nblines*2> arif_length))
+ {
+ endofinspection=true;
+ break;
+ }
+ }
+
+ alif.close();
+ arif.close();
+ areportof.close();
+ return finalreport;
+ }
return SameFile;
}
@@ -603,79 +605,79 @@ namespace PatternGeneratorJRL
prepareDebugFiles();
for (unsigned int lNbIt=0;lNbIt<m_OuterLoopNbItMax;lNbIt++)
- {
- os << "<===============================================================>"<<endl;
- os << "Iteration nb: " << lNbIt << endl;
-
- m_clock.startPlanning();
-
- /*! According to test profile initialize the current profile. */
- chooseTestProfile();
-
- m_clock.endPlanning();
-
- if (m_DebugPR!=0)
- {
- m_DebugPR->currentConfiguration(m_PreviousConfiguration);
- m_DebugPR->currentVelocity(m_PreviousVelocity);
- m_DebugPR->currentAcceleration(m_PreviousAcceleration);
- m_DebugPR->computeForwardKinematics();
- }
-
- bool ok = true;
- while(ok)
- {
- m_clock.startOneIteration();
-
- if (m_PGIInterface==0)
- {
- ok = m_PGI->RunOneStepOfTheControlLoop(m_CurrentConfiguration,
- m_CurrentVelocity,
- m_CurrentAcceleration,
- m_OneStep.ZMPTarget,
- m_OneStep.finalCOMPosition,
- m_OneStep.LeftFootPosition,
- m_OneStep.RightFootPosition);
- }
- else if (m_PGIInterface==1)
- {
- ok = m_PGI->RunOneStepOfTheControlLoop(m_CurrentConfiguration,
- m_CurrentVelocity,
- m_CurrentAcceleration,
- m_OneStep.ZMPTarget);
- }
-
- m_OneStep.NbOfIt++;
-
- m_clock.stopOneIteration();
-
- m_PreviousConfiguration = m_CurrentConfiguration;
- m_PreviousVelocity = m_CurrentVelocity;
- m_PreviousAcceleration = m_CurrentAcceleration;
-
- /*! Call the reimplemented method to generate events. */
- if (ok)
- {
- m_clock.startModification();
- generateEvent();
- m_clock.stopModification();
-
- m_clock.fillInStatistics();
-
-
- /*! Fill the debug files with appropriate information. */
- fillInDebugFiles();
- }
- else
- {
- cerr << "Nothing to dump after " << m_OneStep.NbOfIt << endl;
- }
-
- }
-
- os << endl << "End of iteration " << lNbIt << endl;
- os << "<===============================================================>"<<endl;
- }
+ {
+ os << "<===============================================================>"<<endl;
+ os << "Iteration nb: " << lNbIt << endl;
+
+ m_clock.startPlanning();
+
+ /*! According to test profile initialize the current profile. */
+ chooseTestProfile();
+
+ m_clock.endPlanning();
+
+ if (m_DebugPR!=0)
+ {
+ m_DebugPR->currentConfiguration(m_PreviousConfiguration);
+ m_DebugPR->currentVelocity(m_PreviousVelocity);
+ m_DebugPR->currentAcceleration(m_PreviousAcceleration);
+ m_DebugPR->computeForwardKinematics();
+ }
+
+ bool ok = true;
+ while(ok)
+ {
+ m_clock.startOneIteration();
+
+ if (m_PGIInterface==0)
+ {
+ ok = m_PGI->RunOneStepOfTheControlLoop(m_CurrentConfiguration,
+ m_CurrentVelocity,
+ m_CurrentAcceleration,
+ m_OneStep.ZMPTarget,
+ m_OneStep.finalCOMPosition,
+ m_OneStep.LeftFootPosition,
+ m_OneStep.RightFootPosition);
+ }
+ else if (m_PGIInterface==1)
+ {
+ ok = m_PGI->RunOneStepOfTheControlLoop(m_CurrentConfiguration,
+ m_CurrentVelocity,
+ m_CurrentAcceleration,
+ m_OneStep.ZMPTarget);
+ }
+
+ m_OneStep.NbOfIt++;
+
+ m_clock.stopOneIteration();
+
+ m_PreviousConfiguration = m_CurrentConfiguration;
+ m_PreviousVelocity = m_CurrentVelocity;
+ m_PreviousAcceleration = m_CurrentAcceleration;
+
+ /*! Call the reimplemented method to generate events. */
+ if (ok)
+ {
+ m_clock.startModification();
+ generateEvent();
+ m_clock.stopModification();
+
+ m_clock.fillInStatistics();
+
+
+ /*! Fill the debug files with appropriate information. */
+ fillInDebugFiles();
+ }
+ else
+ {
+ cerr << "Nothing to dump after " << m_OneStep.NbOfIt << endl;
+ }
+
+ }
+
+ os << endl << "End of iteration " << lNbIt << endl;
+ os << "<===============================================================>"<<endl;
+ }
string lProfileOutput= m_TestName;
lProfileOutput +="TimeProfile.dat";
--
GitLab