void ompl::geometric::BasicPRMmodif::reconstructLastSolution(void) { if (lastStart_ && lastGoal_) constructSolution(lastStart_, lastGoal_); else msg_.warn("There is no solution to reconstruct"); }
ompl::base::PlannerStatus ompl::geometric::LazyPRM::solve(const base::PlannerTerminationCondition &ptc) { checkValidity(); base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get()); if (!goal) { OMPL_ERROR("%s: Unknown type of goal", getName().c_str()); return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE; } // Add the valid start states as milestones while (const base::State *st = pis_.nextStart()) startM_.push_back(addMilestone(si_->cloneState(st))); if (startM_.size() == 0) { OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); return base::PlannerStatus::INVALID_START; } if (!goal->couldSample()) { OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str()); return base::PlannerStatus::INVALID_GOAL; } // Ensure there is at least one valid goal state if (goal->maxSampleCount() > goalM_.size() || goalM_.empty()) { const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal(); if (st) goalM_.push_back(addMilestone(si_->cloneState(st))); if (goalM_.empty()) { OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str()); return base::PlannerStatus::INVALID_GOAL; } } unsigned long int nrStartStates = boost::num_vertices(g_); OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates); bestCost_ = opt_->infiniteCost(); base::State *workState = si_->allocState(); std::pair<std::size_t, std::size_t> startGoalPair; base::PathPtr bestSolution; bool fullyOptimized = false; bool someSolutionFound = false; unsigned int optimizingComponentSegments = 0; // Grow roadmap in lazy fashion -- add vertices and edges without checking validity while (ptc == false) { ++iterations_; sampler_->sampleUniform(workState); Vertex addedVertex = addMilestone(si_->cloneState(workState)); const long int solComponent = solutionComponent(&startGoalPair); // If the start & goal are connected and we either did not find any solution // so far or the one we found still needs optimizing and we just added an edge // to the connected component that is used for the solution, we attempt to // construct a new solution. if (solComponent != -1 && (!someSolutionFound || (long int)vertexComponentProperty_[addedVertex] == solComponent)) { // If we already have a solution, we are optimizing. We check that we added at least // a few segments to the connected component that includes the previously found // solution before attempting to construct a new solution. if (someSolutionFound) { if (++optimizingComponentSegments < magic::MIN_ADDED_SEGMENTS_FOR_LAZY_OPTIMIZATION) continue; optimizingComponentSegments = 0; } Vertex startV = startM_[startGoalPair.first]; Vertex goalV = goalM_[startGoalPair.second]; base::PathPtr solution; do { solution = constructSolution(startV, goalV); } while (!solution && vertexComponentProperty_[startV] == vertexComponentProperty_[goalV]); if (solution) { someSolutionFound = true; base::Cost c = solution->cost(opt_); if (opt_->isSatisfied(c)) { fullyOptimized = true; bestSolution = solution; bestCost_ = c; break; } else { if (opt_->isCostBetterThan(c, bestCost_)) { bestSolution = solution; bestCost_ = c; } } } } } si_->freeState(workState); if (bestSolution) { base::PlannerSolution psol(bestSolution); psol.setPlannerName(getName()); // if the solution was optimized, we mark it as such psol.setOptimized(opt_, bestCost_, fullyOptimized); pdef_->addSolutionPath(psol); } OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates); return bestSolution ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT; }
bool ompl::geometric::BasicPRMmodif::solve(const base::PlannerTerminationCondition &ptc) { pis_.checkValidity(); base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get()); if (!goal) { msg_.error("Goal undefined or unknown type of goal"); return false; } std::vector<Milestone*> startM; std::vector<Milestone*> goalM; // add the valid start states as milestones while (const base::State *st = pis_.nextStart()) startM.push_back(addMilestone(si_->cloneState(st))); if (startM.size() == 0) { msg_.error("There are no valid initial states!"); return false; } if (!goal->canSample()) { msg_.error("Insufficient states in sampleable goal region"); return false; } if (!sampler_) sampler_ = si_->allocValidStateSampler(); unsigned int nrStartStates = milestones_.size(); msg_.inform("Starting with %u states", nrStartStates); base::State *xstate = si_->allocState(); std::pair<Milestone*, Milestone*> solEndpoints; while (ptc() == false) { // find at least one valid goal state if (goal->maxSampleCount() > goalM.size()) { const base::State *st = goalM.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal(); if (st) goalM.push_back(addMilestone(si_->cloneState(st))); if (goalM.empty()) { msg_.error("Unable to find any valid goal states"); break; } } // if there already is a solution, construct it // if (haveSolution(startM, goalM, &solEndpoints)) // { // constructSolution(solEndpoints.first, solEndpoints.second); // break; // } // othewise, spend some time building a roadmap else { // if it is worth looking at other goal regions, plan for part of the time if (goal->maxSampleCount() > goalM.size()) growRoadmap(startM, goalM, boost::bind(&growRoadmapTerminationCondition, ptc, time::now() + time::seconds(0.1)), xstate); // otherwise, just go ahead and build the roadmap else growRoadmap(startM, goalM, ptc, xstate); // if a solution has been found, construct it // if (haveSolution(startM, goalM, &solEndpoints)) // { // constructSolution(solEndpoints.first, solEndpoints.second); // break; // } } } if (haveSolution(startM, goalM, &solEndpoints)) { constructSolution(solEndpoints.first, solEndpoints.second); } si_->freeState(xstate); msg_.inform("Created %u states", milestones_.size() - nrStartStates); return goal->isAchieved(); }
int main(int argc, char **argv) { //*****************************SET PARAMETERS************************************************* Parameters params(argc, argv, 0); Parameters params2(argc, argv, 1); //******************************************************************************************** //**********************READ DATA AND MAKE SOLUTION******************************************* Solution* sol = constructSolution(params); preprocesData(sol); Solution* sol2 = constructSolution(params2); preprocesData(sol2); //********************************************************************************************* //*****************************SET PARAMETERS************************************************* if(sol->getNumberOfProcesses() * sol->getNumberOfMachines() <= 100 * 1000) { params2.delta = params.delta = 40; params2.nmb_iters_bpr = params.nmb_iters_bpr = 300; params2.nmbRanges = params.nmbRanges = 7; params2.rangeLength = params.rangeLength = sol->getNumberOfProcesses() / params.nmbRanges; } else { params2.nmb_iters_bpr = params.nmb_iters_bpr = 100; params2.nmbRanges = params.nmbRanges = 5; params2.rangeLength = params.rangeLength = sol->getNumberOfProcesses() / params.nmbRanges; params2.numberOfBestProcessesToConsiderShiftSwap = params.numberOfBestProcessesToConsiderShiftSwap = 3; params2.nmbLoops = params.nmbLoops = 2; if(sol->getNumberOfProcesses() * sol->getNumberOfMachines() >= 20000 * 2000) params2.nmbLoops = params.nmbLoops = 1; } params2.seedValue = params.seedValue + 1000; //******************************************************************************************** if(sol->getNumberOfProcesses() * sol->getNumberOfMachines() <= 100 * 1000) // A instances solveA(params, sol, params2, sol2); else // B instances solveB(params, sol, params2, sol2); long timeToSleep = params.time_limit - (time(0) - params.programStartTime) - 3; if (timeToSleep < 0) timeToSleep = 0; sleep(timeToSleep); // Read Solutions From Files And Write a better one sol->setOriginalLoadCostWeights(); sol2->setOriginalLoadCostWeights(); //cout << " sol->solutionFilename " << sol->solutionFilename << endl; //cout << " sol2->solutionFilename " << sol2->solutionFilename << endl; //cout << " sol->getCost() " << sol->getCost() << endl; //cout << " sol2->getCost() " << sol2->getCost() << endl; readSolutionFromFile( sol, sol->solutionFilename); readSolutionFromFile(sol2, sol2->solutionFilename); //cout << " after sol->getCost() " << sol->getCost() << endl; //cout << " after sol2->getCost() " << sol2->getCost() << endl; if ((sol->getCost() < sol2->getCost()) && (sol->checkConflict(false))) sol->writeToFile(params.solution_filename); else sol2->writeToFile(params.solution_filename); //remove solution files ostringstream oss1(""); oss1 << "rm " << sol->solutionFilename; system(oss1.str().c_str()); ostringstream oss2(""); oss2 << "rm " << sol2->solutionFilename; system(oss2.str().c_str()); //*****************************************FILES (REMOVE THIS PART)******************************************************** /*fstream fileResults("results", ios::out | ios::app); fileResults << setw(5) << params.seedValue << setw(30) << params.data_filename << setw(20) << sol->getCost() << setw(20) << sol2->getCost() << setw(20) << *(params.best_objective) << setw(15) << time(0) - params.programStartTime << endl; // running times fstream fileRunnungTimes("runnung_times", ios::out | ios::app); fileRunnungTimes << setw(5) << setw(30) << params.data_filename << setw(20) << *(params.best_objective) << setw(20) << time(0) - params.programStartTime << endl; // check solution with official checker ostringstream oss(""); oss << "./checker " << params.data_filename << " " << params.initial_assignment_filename << " " << params.solution_filename; oss << " >> checkerFile "; system(oss.str().c_str()); */ //************************************************************************************************************************* return 0; }