Skip to content
Snippets Groups Projects
Commit ed8bd3a1 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Regression test for Herdt2010.

parent b535c10d
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
unitTesting/CMakeLists.txt
+ 4
− 0
View file @ ed8bd3a1
......@@ -167,6 +167,8 @@ ADD_EXECUTABLE( TestHerdt2010
../src/TimeUtilsWindows.cpp
TestHerdt2010.cpp
CommonTools.cpp
TestObject.cpp
ClockCPUTime.cpp
)
SET_TARGET_PROPERTIES( TestHerdt2010
PROPERTIES
......@@ -176,6 +178,8 @@ SET_TARGET_PROPERTIES( TestHerdt2010
target_link_libraries (TestHerdt2010 ${PROJECT_NAME})
ADD_DEPENDENCIES (TestHerdt2010 ${PROJECT_NAME})
ADD_TEST(TestHerdt2010 TestHerdt2010 ${samplemodelpath} sample.wrl ${samplespec} ${sampleljr} ${sampleinitconfig})
#####################
# Add user examples #
#####################
......
unitTesting/TestHerdt2010.cpp
+ 92
− 314
View file @ ed8bd3a1
......@@ -25,351 +25,129 @@
/* \file This file tests A. Herdt's walking algorithm for
* automatic foot placement giving an instantaneous CoM velocity reference.
*/
#include "TestFootPrintPGInterfaceData.h"
#include "CommonTools.h"
#include "TestObject.h"
using namespace::PatternGeneratorJRL;
using namespace::PatternGeneratorJRL::TestSuite;
using namespace std;
void Herdt(PatternGeneratorInterface &aPGI)
{
CommonInitialization(aPGI);
enum Profiles_t {
PROFIL_HERDT_ONLINE_WALKING, // 1
};
{
istringstream strm2(":setVelReference 0.2 0.0 0.0");
aPGI.ParseCmd(strm2);
}
{
istringstream strm2(":singlesupporttime 0.7");
aPGI.ParseCmd(strm2);
}
{
istringstream strm2(":SetAlgoForZmpTrajectory Herdt");
aPGI.ParseCmd(strm2);
}
class TestHerdt2010: public TestObject
{
if (0)
private:
public:
TestHerdt2010(int argc, char *argv[], string &aString, int TestProfile):
TestObject(argc,argv,aString)
{
istringstream strm2(":setdimitrovconstraint XY 0.07 0.05");
aPGI.ParseCmd(strm2);
}
m_TestProfile = TestProfile;
};
protected:
void startOnLineWalking(PatternGeneratorInterface &aPGI)
{
istringstream strm2(":stepseq 0.0 -0.105 0.0 \
0.2 0.21 0.0 \
0.2 -0.21 0.0 \
0.2 0.21 0.0 \
0.2 -0.21 0.0 \
0.2 0.21 0.0 \
0.0 -0.21 0.0");
aPGI.ParseCmd(strm2);
CommonInitialization(aPGI);
{
istringstream strm2(":SetAlgoForZmpTrajectory Herdt");
aPGI.ParseCmd(strm2);
}
{
istringstream strm2(":singlesupporttime 0.7");
aPGI.ParseCmd(strm2);
}
{
istringstream strm2(":HerdtOnline 0.2 0.0 0.0");
aPGI.ParseCmd(strm2);
}
{
istringstream strm2(":numberstepsbeforestop 2");
aPGI.ParseCmd(strm2);
}
}
}
void Herdt_Stop(PatternGeneratorInterface &aPGI)
{
void stopOnLineWalking(PatternGeneratorInterface &aPGI)
{
istringstream strm2(":setVelReference 0.0 0.0 0.0");
aPGI.ParseCmd(strm2);
{
istringstream strm2(":setVelReference 0.0 0.0 0.0");
aPGI.ParseCmd(strm2);
}
}
}
void HerdtOnline(PatternGeneratorInterface &aPGI)
{
CommonInitialization(aPGI);
void chooseTestProfile()
{
istringstream strm2(":SetAlgoForZmpTrajectory Herdt");
aPGI.ParseCmd(strm2);
}
{
istringstream strm2(":singlesupporttime 0.7");
aPGI.ParseCmd(strm2);
}
{
istringstream strm2(":HerdtOnline 0.2 0.0 0.0");
aPGI.ParseCmd(strm2);
switch(m_TestProfile)
{
case PROFIL_HERDT_ONLINE_WALKING:
startOnLineWalking(*m_PGI);
break;
default:
throw("No correct test profile");
break;
}
}
void generateEvent()
{
istringstream strm2(":numberstepsbeforestop 2");
aPGI.ParseCmd(strm2);
}
}
unsigned int StoppingTime = 11*200;
if (m_OneStep.NbOfIt>StoppingTime)
{
stopOnLineWalking(*m_PGI);
}
}
};
int main(int argc, char *argv[])
int PerformTests(int argc, char *argv[])
{
unsigned int TestProfil=PROFIL_HERDT_ONLINE;
string VRMLPath;
string VRMLFileName;
string SpecificitiesFileName;
string LinkJointRank;
string InitConfig;
MAL_VECTOR_DIM(CurrentConfiguration,double,46);
MAL_VECTOR_DIM(CurrentVelocity,double,46);
MAL_VECTOR_DIM(CurrentAcceleration,double,46);
MAL_VECTOR_DIM(PreviousConfiguration,double,46) ;
MAL_VECTOR_DIM(PreviousVelocity,double,46);
MAL_VECTOR_DIM(PreviousAcceleration,double,46);
getOptions(argc,argv,
VRMLPath, VRMLFileName,
SpecificitiesFileName, LinkJointRank,
InitConfig,
TestProfil);
// Humanoids
CjrlHumanoidDynamicRobot * aHDR = 0, * aDebugHDR = 0;
// Create Pattern Generator Interface
PatternGeneratorInterface * aPGI=0;
// Instanciate and initialize.
string RobotFileName = VRMLPath + VRMLFileName;
CreateAndInitializeHumanoidRobot(RobotFileName,
SpecificitiesFileName,
LinkJointRank, InitConfig,
aHDR, aDebugHDR, aPGI);
// Specify the walking mode: here the default one.
istringstream strm2(":walkmode 0");
aPGI->ParseCmd(strm2);
MAL_VECTOR_DIM(ZMPTarget,double,3);
//COMPosition CurrentWaistPosition;
struct timeval begin,end,startingtime;
unsigned long int NbOfIt=0, NbOfItToCompute=0;
COMPosition finalCOMPosition;
FootAbsolutePosition LeftFootPosition;
FootAbsolutePosition RightFootPosition;
std::string TestNames[1] = { "TestHerdt2010OnLine"};
int TestProfiles[1] = { PROFIL_HERDT_ONLINE_WALKING};
bool DebugFGPI = true;
bool DebugZMP2 = true;
unsigned int PGIInterface = 0;
double TimeProfile[200*620];
bool bTimeProfile=true;
double TimeProfileTS[200*620];
unsigned int TimeProfileIndex = 0;
unsigned int TimeProfileUpperLimit=200*620;
ofstream aofzmpmb2;
if (DebugZMP2)
aofzmpmb2.open("ZMPMBSTAGE2.dat",ofstream::out);
ofstream aof;
if (DebugFGPI)
{
aof.open("TestFGPI_description.dat",ofstream::out);
string Titles[26] =
{ "Time",
"Com X",
"Com Y" ,
"Com Z" ,
"Com Yaw",
"Com dX" ,
"Com dY" ,
"Com dZ" ,
"ZMP X (waist ref.)" ,
"ZMP Y (waist ref.)" ,
"Left Foot X" ,
"Left Foot Y" ,
"Left Foot Z" ,
"Left Foot Theta" ,
"Left Foot Omega" ,
"Left Foot Omega2" ,
"Right Foot X" ,
"Right Foot Y" ,
"Right Foot Z" ,
"Right Foot Theta" ,
"Right Foot Omega" ,
"Right Foot Omega2" ,
"ZMP X (world ref.)" ,
"ZMP Y (world ref.)" ,
"Waist X (world ref.)" ,
"Waist Y (world ref.)" };
for(unsigned int i=0;i<26;i++)
aof << i+1 << ". " <<Titles[i] <<std::endl;
aof.close();
aof.open("TestFGPI.dat",ofstream::out);
}
double totaltime=0,maxtime=0;
double totaltimemodif=0, timemodif = 0;
double totaltimeinplanning=0;
unsigned long int nbofmodifs=0;
gettimeofday(&startingtime,0);
// Number of sequences added.
unsigned int lNbItMax = 1;
for (unsigned int lNbIt=0;lNbIt<lNbItMax;lNbIt++)
for (unsigned int i=0;i<1;i++)
{
//StrangeStartingPosition(*aPGI);
cout << "<===============================================================>"<<endl;
cout << "Iteration nb: " << lNbIt << endl;
gettimeofday(&begin,0);
switch (TestProfil)
{
case PROFIL_HERDT:
Herdt(*aPGI);
break;
case PROFIL_HERDT_ONLINE:
HerdtOnline(*aPGI);
break;
default:
break;
};
// Should generate the same than the one previous (but shorter to specify).
gettimeofday(&end,0);
double ltime = end.tv_sec-begin.tv_sec + 0.000001 * (end.tv_usec - begin.tv_usec);
totaltimeinplanning+=ltime;
aDebugHDR->currentConfiguration(PreviousConfiguration);
aDebugHDR->currentVelocity(PreviousVelocity);
aDebugHDR->currentAcceleration(PreviousAcceleration);
aDebugHDR->computeForwardKinematics();
bool ok = true;
while(ok)
TestHerdt2010 aTH2010(argc,argv,
TestNames[i],
TestProfiles[i]);
try
{
gettimeofday(&begin,0);
if (PGIInterface==0)
{
ok = aPGI->RunOneStepOfTheControlLoop(CurrentConfiguration,
CurrentVelocity,
CurrentAcceleration,
ZMPTarget,
finalCOMPosition,
LeftFootPosition,
RightFootPosition);
}
else if (PGIInterface==1)
{
ok = aPGI->RunOneStepOfTheControlLoop(CurrentConfiguration,
CurrentVelocity,
CurrentAcceleration,
ZMPTarget);
}
gettimeofday(&end,0);
double ltime = end.tv_sec-begin.tv_sec + 0.000001 * (end.tv_usec - begin.tv_usec);
if (maxtime<ltime)
maxtime = ltime;
NbOfIt++;
if (ltime>0.000300)
{
totaltime += ltime;
NbOfItToCompute++;
}
PreviousConfiguration = CurrentConfiguration;
PreviousVelocity = CurrentVelocity;
PreviousAcceleration = CurrentAcceleration;
timemodif =0;
if (TestProfil==PROFIL_HERDT_ONLINE)
if (!aTH2010.doTest(std::cout))
{
if (NbOfIt>11*200) /* 200 iterations per second */
{
Herdt_Stop(*aPGI);
if(NbOfIt > 10*200)
ok=false;
}
if ((NbOfIt>2.5*200) && (NbOfIt<2.75*200)) /* */
{
// aPGI->setCoMPerturbationForce(50,0.0);
}
}
TimeProfile[TimeProfileIndex] = ltime + timemodif;
TimeProfileTS[TimeProfileIndex] = begin.tv_sec + 0.000001 * begin.tv_usec;
TimeProfileIndex++;
if (TimeProfileIndex>TimeProfileUpperLimit)
TimeProfileIndex = 0;
if (DebugFGPI)
{
aof << NbOfIt*0.005 << " "
<< finalCOMPosition.x[0] << " "
<< finalCOMPosition.y[0] << " "
<< finalCOMPosition.z[0] << " "
<< finalCOMPosition.yaw << " "
<< finalCOMPosition.x[1] << " "
<< finalCOMPosition.y[1] << " "
<< finalCOMPosition.z[1] << " "
<< ZMPTarget(0) << " " << ZMPTarget(1) << " "
<< LeftFootPosition.x << " " << LeftFootPosition.y << " "
<< LeftFootPosition.z << " " << LeftFootPosition.theta << " "
<< LeftFootPosition.omega << " " << LeftFootPosition.omega2 << " "
<< RightFootPosition.x << " " << RightFootPosition.y << " "
<< RightFootPosition.z << " " << RightFootPosition.theta << " "
<< RightFootPosition.omega << " " << RightFootPosition.omega2 << " "
<< ZMPTarget(0)*cos(CurrentConfiguration(5)) -
ZMPTarget(1)*sin(CurrentConfiguration(5))
+CurrentConfiguration(0) << " "
<< ZMPTarget(0)*sin(CurrentConfiguration(5)) +
ZMPTarget(1)*cos(CurrentConfiguration(5))
+CurrentConfiguration(1) << " "
<< CurrentConfiguration(0) << " "
<< CurrentConfiguration(1) << " "
<< begin.tv_sec + 0.000001 * begin.tv_usec
<< endl;
cout << "Failed test " << i << endl;
return -1;
}
else
cout << "Passed test " << i << endl;
}
cout << "End of iteration " << lNbIt << endl;
cout << "<===============================================================>"<<endl;
catch (const char * astr)
{ cerr << "Failed on following error " << astr << std::endl;
return -1; }
}
return 0;
}
int main(int argc, char *argv[])
{
try
{
return PerformTests(argc,argv);
}
catch (const std::string& msg)
{
std::cerr << msg << std::endl;
}
return 1;
}
aofzmpmb2.close();
if (bTimeProfile)
{
ofstream lProfileOutput("TimeProfile.dat",ofstream::out);
double dST = startingtime.tv_sec + 0.000001 * startingtime.tv_usec;
for(unsigned int i=0;i<TimeProfileIndex;i++)
lProfileOutput << " " << TimeProfileTS[i] - dST
<< " " << TimeProfile[i] << std::endl;
lProfileOutput.close();
}
if (DebugFGPI)
aof.close();
delete aPGI;
cout << "Number of iterations " << NbOfIt << " " << NbOfItToCompute << endl;
cout << "Time consumption: " << (double)totaltime/(double)NbOfItToCompute
<< " max time: " << maxtime <<endl;
cout << "Time for modif: " << (double)totaltimemodif/(double)nbofmodifs
<< " nb of modifs: " << nbofmodifs << endl ;
cout << "Time on ZMP ref planning (Kajita policy): "
<< totaltimeinplanning<< " "
<< totaltimeinplanning*4/(double)NbOfIt<< endl;
}
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