void PointToSetMotionPlanner::GetSolution(MilestonePath& path)
{
path.edges.clear();
for(size_t i=0;i<goalNodes.size();i++) {
MilestonePath temp;
mp->GetPath(0,goalNodes[i],temp);
if(!temp.edges.empty()) {
if(path.edges.empty()) path = temp;
else {
if(temp.Length() < path.Length())
path = temp;
}
}
}
}
std::string MotionPlannerInterface::Plan(MilestonePath& path,const HaltingCondition& cond)
{
path.edges.clear();
bool foundPath = false;
Real lastCheckTime = 0, lastCheckValue = 0;
Timer timer;
for(int iters=0;iters<cond.maxIters;iters++) {
Real t=timer.ElapsedTime();
if(t > cond.timeLimit) {
if(foundPath) {
//get the final path
GetSolution(path);
}
return "timeLimit";
}
//check for cost improvements
if(foundPath && t > lastCheckTime + cond.costImprovementPeriod) {
GetSolution(path);
Real len = path.Length();
if(len < cond.costThreshold)
return "costThreshold";
if(lastCheckValue - len < cond.costImprovementThreshold)
return "costImprovementThreshold";
lastCheckTime = t;
lastCheckValue = len;
}
//do planning, check if a path is found
PlanMore();
if(!foundPath) {
if(IsSolved()) {
foundPath = true;
GetSolution(path);
if(cond.foundSolution) {
return "foundSolution";
}
lastCheckTime = t;
lastCheckValue = path.Length();
}
}
}
if(foundPath) {
//get the final path
GetSolution(path);
}
return "maxIters";
}
ob::PlannerStatus KrisLibraryOMPLPlanner::solve (const ob::PlannerTerminationCondition &ptc)
{
if(!planner) {
//may have had a previous clear() call
//fprintf(stderr,"KrisLibraryOMPLPlanner::solve(): Warning, setup() not called yet\n");
setup();
if(!planner)
return ob::PlannerStatus(ob::PlannerStatus::UNKNOWN);
}
ob::ProblemDefinitionPtr pdef = this->getProblemDefinition();
//how much to plan?
int increment = (StartsWith(factory.type.c_str(),"fmm") ? 1 : 50);
Real oldBest = Inf;
bool optimizing = planner->IsOptimizing();
double desiredCost = 0;
if(optimizing) {
if(pdef->getOptimizationObjective() != NULL)
desiredCost = pdef->getOptimizationObjective()->getCostThreshold().value();
else
OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.", getName().c_str());
}
while(!ptc()) {
if(planner->IsSolved()) {
//convert solution to OMPL solution
MilestonePath path;
planner->GetSolution(path);
if(optimizing) {
//optimizing
Real cost = path.Length();
if(cost < oldBest) {
oldBest = cost;
if(cost < desiredCost) {
ob::PathPtr pptr(ToOMPL(si_,path));
this->getProblemDefinition()->addSolutionPath(pptr);
return ob::PlannerStatus(true,false);
}
}
}
else {
//non-optimizing
ob::PathPtr pptr(ToOMPL(si_,path));
this->getProblemDefinition()->addSolutionPath(pptr);
return ob::PlannerStatus(true,false);
}
}
planner->PlanMore(increment);
}
if(planner->IsSolved()) {
//convert solution to OMPL solution
MilestonePath path;
planner->GetSolution(path);
ob::PathPtr pptr(ToOMPL(si_,path));
this->getProblemDefinition()->addSolutionPath(pptr);
return ob::PlannerStatus(true,false);
}
return ob::PlannerStatus(false,false);
}