void testAll(int pos)
{
if (Clock::now() >= timeout)
return;
if (pos == agents.size())
{
vector<State> starts;
for (Agent& agent : agents)
starts.push_back(*agent.foundIt);
testSolution(starts, false);
return;
}
if (agents[pos].foundIt == agents[pos].found.cend())
return;
testAll(pos+1);
++agents[pos].foundIt;
testAll(pos);
--agents[pos].foundIt;
}
int main(int argc, char** argv)
{
//load map
readMap();
num_expands = 0;
num_discovered = 1;
//run planner
test_duration = 0;
Clock::time_point t0 = Clock::now();
timeout = t0 + chrono::seconds(5);
prepare();
search();
Clock::time_point t1 = Clock::now();
//report joint plan
testSolution(joint_solution, true);
//report stats
double dt = chrono::duration<double, chrono::seconds::period>(t1-t0).count();
cout << "results: Path Length=" << joint_cost/10000.0;
cout << " Visited Nodes=" << num_discovered;
cout << " Explored Nodes=" << num_expands;
cout << " Planning Time=" << dt << endl;
cout << " Joint Testing Time=" << test_duration << endl;
FILE* fout = fopen("stats.csv","w");
fprintf(fout,"%d %f %d %f %f\n",W,joint_cost/10000.0,num_expands,dt,test_duration);
fclose(fout);
for (int a = 0; a < agents.size(); ++a)
{
fout = fopen("visualf.txt","w");
vector<string> visual = visualizePath();
for (int i = 0; i < visual.size(); ++i)
fprintf(fout,"%s\n",visual[i].c_str());
fclose(fout);
}
}
void HCOD::activeSearch( VectorXd & u )
{
// if( isDebugOnce ) { sotDebugTrace::openFile(); isDebugOnce = false; }
// else { if(sotDEBUGFLOW.outputbuffer.good()) sotDebugTrace::closeFile(); }
//if(sotDEBUGFLOW.outputbuffer.good()) { sotDebugTrace::closeFile();sotDebugTrace::openFile(); }
sotDEBUGIN(15);
/*
* foreach stage: stage.initCOD(Ir_init)
* u = 0
* u0 = solve
* do
* tau,cst_ref = max( violation(stages) )
* u += (1-tau)u0 + tau*u1
* if( tau<1 )
* update(cst_ref); break;
*
* lambda,w = computeLambda
* cst_ref,lmin = min( lambda,w )
* if lmin<0
* downdate( cst_ref )
*
*/
assert(VectorXi::LinSpaced(3,0,2)[0] == 0
&& VectorXi::LinSpaced(3,0,2)[1] == 1
&& VectorXi::LinSpaced(3,0,2)[2] == 2
&& "new version of Eigen might have change the "
"order of arguments in LinSpaced, please correct");
/*struct timeval t0,t1,t2;double time1,time2;
gettimeofday(&t0,NULL);*/
initialize();
sotDEBUG(5) << "Y= " << (MATLAB)Y.matrixExplicit<< std::endl;
Y.computeExplicitly(); // TODO: this should be done automatically on Y size.
sotDEBUG(5) << "Y= " << (MATLAB)Y.matrixExplicit<< std::endl;
/*gettimeofday(&t1,NULL);
time1 = ((t1.tv_sec-t0.tv_sec)+(t1.tv_usec-t0.tv_usec)/1.0e6);*/
int iter = 0;
startTime=getCPUtime();
Index stageMinimal = 0;
do
{
iter ++; sotDEBUG(5) << " --- *** \t" << iter << "\t***.---" << std::endl;
//if( iter>1 ) { break; }
if( sotDEBUG_ENABLE(15) ) show( sotDEBUGFLOW );
assert( testRecomposition(&std::cerr) );
damp();
computeSolution();
assert( testSolution(&std::cerr) );
double tau = computeStepAndUpdate();
if( tau<1 )
{
sotDEBUG(5) << "Update done, make step <1." << std::endl;
makeStep(tau);
}
else
{
sotDEBUG(5) << "No update, make step ==1." << std::endl;
makeStep();
for( ;stageMinimal<=(Index)stages.size();++stageMinimal )
{
sotDEBUG(5) << "--- Started to examinate stage " << stageMinimal << std::endl;
computeLagrangeMultipliers(stageMinimal);
if( sotDEBUG_ENABLE(15) ) show( sotDEBUGFLOW );
//assert( testLagrangeMultipliers(stageMinimal,std::cerr) );
if( searchAndDowndate(stageMinimal) )
{
sotDEBUG(5) << "Lagrange<0, downdate done." << std::endl;
break;
}
for( Index i=0;i<stageMinimal;++i )
stages[i]->freezeSlacks(false);
if( stageMinimal<nbStages() )
stages[stageMinimal]->freezeSlacks(true);
}
}
lastTime=getCPUtime()-startTime;
lastNumberIterations=iter;
if( lastTime>maxTime ) throw 667;
if( iter>maxNumberIterations ) throw 666;
} while(stageMinimal<=nbStages());
sotDEBUG(5) << "Lagrange>=0, no downdate, active search completed." << std::endl;
/*gettimeofday(&t2,NULL);
time2 = ((t2.tv_sec-t1.tv_sec)+(t2.tv_usec-t1.tv_usec)/1.0e6);
std::ofstream fup("/tmp/haset.dat",std::ios::app);
fup << time1<<"\t"<<time2<<"\t"<<iter<<"\t";*/
u=solution;
sotDEBUG(5) << "uf =" << (MATLAB)u << std::endl;
sotDEBUGOUT(15);
}
