/**
* Uniform Cost Search
*/
void UCS(const vector<string> &ground)
{
int time1 = clock();
Statistics stat;
priority_queue<UCSState>q;
StateSet rec;
UCSState init(0);
initState(ground, init.state);
vector<State>stateVector;
q.push(init);
stateVector.push_back(init.state);
rec.insert(init.state);
UCSState result;
while (!q.empty()) {
UCSState tmp = q.top();
q.pop();
for (int i = 0; i < 4; ++i) {
UCSState now = tmp;
now.state.person.x += direction[i][0];
now.state.person.y += direction[i][1];
int s = validState(direction[i][0], direction[i][1], now.state, ground);
now.state.move = step[i];
now.state.previousStateNum = now.state.currentStateNum;
stat.anodes++;
if (s == -1) {
result = now;
goto end;
} else if (s && !rec.count(now.state)) {
now.cost += s;
now.state.currentStateNum = stateVector.size();
stateVector.push_back(now.state);
rec.insert(now.state);
q.push(now);
} else if (s) {
stat.bnodes++;
}
}
}
end:
stat.cnodes = q.size();
stat.dnodes = rec.size() + 1;
stat.runtime = (clock() - time1) * 1.0 / CLOCKS_PER_SEC;
outputStat(stat);
outputSolution(stateVector, result.state);
}
/**
* dfs to the solution
*/
void DFS(const vector<string> &ground)
{
int time1 = clock();
Statistics stat;
State init;
initState(ground, init);
StateSet rec;
vector<State>stateVector;
stateVector.push_back(init);
int flg = 0;
State result;
stack<State>st;
st.push(init);
while (!st.empty()) {
State tmp = st.top();
st.pop();
for (int i = 0; i < 4; ++i) {
State now = tmp;
now.person.x += direction[i][0];
now.person.y += direction[i][1];
int s = validState(direction[i][0], direction[i][1], now, ground);
now.move = step[i];
now.previousStateNum = now.currentStateNum;
stat.anodes++;
if (s == -1) {
result = now;
goto end;
} else if (s && !rec.count(now)) {
now.currentStateNum = stateVector.size();
st.push(now);
stateVector.push_back(now);
rec.insert(now);
} else if (s) {
stat.bnodes++;
}
}
}
end:
stat.cnodes = st.size();
stat.dnodes = rec.size() + 1;
stat.runtime = (clock() - time1) * 1.0 / CLOCKS_PER_SEC;
outputStat(stat);
outputSolution(stateVector, result);
}
// Runs [numSims] of the given solver and and returns the results
// (i.e., expectedCost, variance, totalTime, statesSeen).
// Argument [algorithm] is the name of the algorithm implemented by [solver].
// Argument [maxTime], if set to > 0, specifies the maximum time allowed to
// the algorithm to complete all simulations (in milliseconds).
// If [perReplan] is passed, then [maxTime] is used as the maximum time allowed
// per re-planning event.
vector<double> simulate(Solver* solver,
string algorithm,
int numSims,
int maxTime = -1,
bool perReplan = false)
{
double expectedCost = 0.0;
double variance = 0.0;
double totalTime = 0.0;
double longestTime = 0.0;
double expectedTime = 0.0; // expected *total* time
double varianceTime = 0.0; // variance *total* time
StateSet statesSeen;
int cnt = 0;
int numDecisions = 0;
clock_t simulationsStartTime = clock();
for (int i = 0; i < numSims; i++) {
if (verbosity >= 100)
cout << " ********* Simulation Starts ********* " << endl;
clock_t startTime, endTime;
double simulationPlanTime = 0.0;
// If requested, reset all state information computed by the algorithm
if (mustResetPlanner(i)) {
for (State* s : problem->states())
s->reset();
if (maxTime > 0) {
solver->maxPlanningTime(maxTime);
}
solver->reset();
if (!flag_is_registered("precompute-h"))
heuristic->reset();
startTime = clock();
// Initial planning
if (algorithm != "greedy")
solver->solve(problem->initialState());
endTime = clock();
double planTime = (double(endTime - startTime) / CLOCKS_PER_SEC);
totalTime += planTime;
simulationPlanTime += planTime;
longestTime = std::max(longestTime, planTime);
numDecisions++;
}
if (verbosity >= 10) {
cout << "Starting simulation " << i << endl;
}
State* tmp = problem->initialState();
if (verbosity >= 100) {
cout << "Estimated cost " <<
problem->initialState()->cost() << endl;
}
// This is where the actual simulated trial starts
double costTrial = 0.0;
int plausTrial = 0;
while (!problem->goal(tmp)) {
statesSeen.insert(tmp);
Action* a;
// Re-planning
if (mustReplan(solver, algorithm, tmp, plausTrial)) {
startTime = clock();
int simulationsElapsedTime =
std::ceil(1000 * (double(startTime - simulationsStartTime)
/ CLOCKS_PER_SEC));
if (maxTime > -1) {
int planningTime = perReplan ?
maxTime : std::max(0, maxTime - simulationsElapsedTime);
solver->maxPlanningTime(planningTime);
}
if (algorithm != "greedy")
a = solver->solve(tmp);
dprint("found action" , (void *) a);
endTime = clock();
double planTime =
(double(endTime - startTime) / CLOCKS_PER_SEC);
totalTime += planTime;
simulationPlanTime += planTime;
longestTime = std::max(longestTime, planTime);
numDecisions++;
if (algorithm != "hop")
a = greedyAction(problem, tmp);
} else {
if (useUpperBound) {
// The algorithms that use upper bounds store the
// greedy action with respect to the upper bound
// in State::bestAction_
a = tmp->bestAction();
}
else {
a = greedyAction(problem, tmp);
}
}
if (verbosity >= 1000) {
cout << "State/Action: " << tmp << " " << a << " " << endl;
}
costTrial += problem->cost(tmp, a);
costTrial = std::min(costTrial, mdplib::dead_end_cost);
if (costTrial >= mdplib::dead_end_cost) {
break;
}
double prob = 0.0;
State* aux = randomSuccessor(problem, tmp, a, &prob);
if (algorithm == "hdp") {
double maxProb = 0.0;
for (auto const & sccr : problem->transition(tmp, a))
maxProb = std::max(maxProb, sccr.su_prob);
plausTrial +=
static_cast<HDPSolver*>(solver)->kappa(prob, maxProb);
}
tmp = aux;
}
if (verbosity >= 10)
cout << costTrial << endl;
if (flag_is_registered("ctp")) {
CTPState* ctps = static_cast<CTPState*>(tmp);
if (!ctps->badWeather()) {
cnt++;
updateStatistics(costTrial, cnt, expectedCost, variance);
updateStatistics(
simulationPlanTime, cnt, expectedTime, varianceTime);
}
} else {
cnt++;
updateStatistics(costTrial, cnt, expectedCost, variance);
updateStatistics(
simulationPlanTime, cnt, expectedTime, varianceTime);
}
if (verbosity >=0) {
if (cnt % 500 == 0 || i == numSims - 1) {
double reportedTime = perReplan ?
totalTime / numDecisions : totalTime;
cout << "sim " << cnt << " exp.cost " << expectedCost
<< " var " << variance / (cnt - 1)
<< " time " << reportedTime
<< " longestTime " << longestTime << " "
<< " Exp[total time] " << expectedTime
<< " Var[total time] " << varianceTime / (cnt - 1) << endl;
}
}
}
double reportedTime = perReplan ? totalTime / numDecisions : totalTime;
if (verbosity >= 10) {
cout << "Estimated cost " << problem->initialState()->cost() << " ";
cout << "Avg. Exec cost " << expectedCost << " ";
cout << "Std. Dev. " << sqrt(variance / (cnt - 1)) << " ";
cout << "Total time " << totalTime / cnt << " " << endl;
cout << "States seen " << statesSeen.size() << endl;
cout << "Avg. time per decision "
<< totalTime / numDecisions << endl
<< "Longest planning time " << longestTime << endl;
cout << "Num. decisions " << numDecisions << endl;
}
double results[] = {expectedCost,
variance / (cnt - 1),
reportedTime,
longestTime};
return vector<double>(results, results + sizeof(results) / sizeof(double));
}
