예제 #1
0
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;
      }
    }
  }   
}
예제 #2
0
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";
}
예제 #3
0
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);
}