void EGraphManager<HeuristicType>::precomputeTSPcosts(){
unsigned int num_TSPs = pow(2, numgoals_)-1; // powerset of set of goals
// For each agent,
// compute distances between goals for each agent,
// and solve the open-loop TSP starting at each goal
for(int agent_i = 0; agent_i < numagents_; agent_i++){
Matrix goalDistances;
computeDistanceBetweenGoals(agent_i, goalDistances);
const int maxnumgoals = MAXNUMGOALS;
for(unsigned long int TSP_i = 0; TSP_i < num_TSPs; TSP_i ++){
std::bitset<maxnumgoals> TSP_i_vertices (TSP_i+1);
#ifdef DEBUG_PRECOMP_HEUR
std::cout<<"Computing TSP for assignment "<<TSP_i_vertices<<std::endl;
#endif
std::vector<int> goalindices;
// find indices of goals corresponding to this assignment
for(int goal_i = 0; goal_i < numgoals_; goal_i++){
if(TSP_i_vertices.test(goal_i))
goalindices.push_back(goal_i);
}
std::vector<int> costV;
solveTSP(goalDistances, goalindices, costV);
TSP_allagents_[agent_i][TSP_i] = costV;
#ifdef DEBUG_PRECOMP_HEUR
SBPL_INFO("TSPs for agent %d", agent_i);
for(int i = 0; i < (int)costV.size(); i++)
printf("%d ", costV[i]);
printf("\n");
#endif
}
}
}
void EGraphManager<HeuristicType>::solveTSP(const Matrix& edgeCosts,
const std::vector<int>& vertex_indices,
std::vector<int>& costV) const{
//TODO: Will call TSP solver. For now, enumerate. Also, does not use fact that
// costs are symmetric
#ifdef DEBUG_PRECOMP_HEUR
SBPL_INFO("solving TSPs for goal assignment:");
for(int i = 0; i < (int) vertex_indices.size(); i++)
printf("%d ", vertex_indices[i]);
printf("\n");
#endif
costV.assign(vertex_indices.size(), 0);
if(vertex_indices.size() < 2)
return;
int heur = 0;
// starting at each vertex, solve TSP optimally
for(unsigned int start_i = 0; start_i < vertex_indices.size(); start_i++){
int bestheursofar = std::numeric_limits<int>::max();
int startvertex = vertex_indices[start_i];
// consider all possible permutations of other goals
std::vector<int> nonstart_vertex_indices = vertex_indices;
nonstart_vertex_indices.erase(nonstart_vertex_indices.begin()+start_i);
do{
// cost from startvertex to first goal
heur = edgeCosts[startvertex][nonstart_vertex_indices[0]];
#ifdef DEBUG_PRECOMP_HEUR
printf("start_i = %d startvertex = %d Non-start vertices =", start_i, startvertex);
for(int i = 0; i < (int) nonstart_vertex_indices.size(); i++)
printf("%d ", nonstart_vertex_indices[i]);
printf("\n");
#endif
// add costs from each goal to the next
for(int i = 0; i < (int) nonstart_vertex_indices.size()-1; i++){
heur += edgeCosts[nonstart_vertex_indices[i]][nonstart_vertex_indices[i+1]];
if(heur > bestheursofar)
break;
}
if(heur < bestheursofar)
bestheursofar = heur;
}while(std::next_permutation(nonstart_vertex_indices.begin(), nonstart_vertex_indices.end()));
costV[start_i] = bestheursofar;
}
#ifdef DEBUG_PRECOMP_HEUR
printf("\n");
std::cin.get();
#endif
}
void PlanningEnvironmentMPH::readMotionPrimitives(const std::string& file)
{
FILE* fMotPrims = fopen(file.c_str(), "r");
if(fMotPrims == NULL)
{
SBPL_ERROR("ERROR: unable to open %s\n", file.c_str());
throw new SBPL_Exception();
}
char sTemp[1024], sExpected[1024];
float fTemp;
int dTemp;
int totalNumofActions = 0;
SBPL_PRINTF("Reading in motion primitives...");
//read in the resolution
strcpy(sExpected, "resolution_m:");
if (fscanf(fMotPrims, "%s", sTemp) == 0)
throw new SBPL_Exception();
if (strcmp(sTemp, sExpected) != 0)
{
SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
throw new SBPL_Exception();
}
if (fscanf(fMotPrims, "%f", &fTemp) == 0)
throw new SBPL_Exception();
double resolutionCorrectionFactor = params::motionPrimitiveScaleFactor;
if (fabs(fTemp - params::motionPrimitiveResolution) > ERR_EPS)
{
resolutionCorrectionFactor = params::motionPrimitiveResolution*params::motionPrimitiveScaleFactor / fTemp;
SBPL_INFO("resolution %f (instead of %f) in the dynamics file; scaling numerical values by %0.3f.\n", fTemp, State::cellSize.xy, resolutionCorrectionFactor);
}
//read in the angular resolution
strcpy(sExpected, "numberofangles:");
if (fscanf(fMotPrims, "%s", sTemp) == 0)
throw new SBPL_Exception();
if (strcmp(sTemp, sExpected) != 0)
{
SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
throw new SBPL_Exception();
}
if (fscanf(fMotPrims, "%d", &dTemp) == 0)
throw new SBPL_Exception();
if (dTemp != State::cellSize.yaw_count)
{
SBPL_ERROR("ERROR: invalid angular resolution %d angles (instead of %d angles) in the motion primitives file\n", dTemp, State::cellSize.yaw_count);
throw new SBPL_Exception();
}
//read in the total number of actions
strcpy(sExpected, "totalnumberofprimitives:");
if (fscanf(fMotPrims, "%s", sTemp) == 0)
throw new SBPL_Exception();
if (strcmp(sTemp, sExpected) != 0)
{
SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
throw new SBPL_Exception();
}
if (fscanf(fMotPrims, "%d", &totalNumofActions) == 0)
{
throw new SBPL_Exception();
}
for (int i = 0; i < totalNumofActions; i++)
{
SBPL_xytheta_mprimitive motprim;
if (readinMotionPrimitive(&motprim, fMotPrims, resolutionCorrectionFactor) == false)
throw new SBPL_Exception();
motionPrimitives.push_back(motprim);
}
fclose(fMotPrims);
precomputeActionswithCompleteMotionPrimitive(&motionPrimitives);
}
