ompl::base::PlannerStatus ompl::control::SST::solve(const base::PlannerTerminationCondition &ptc) { checkValidity(); base::Goal *goal = pdef_->getGoal().get(); base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal); while (const base::State *st = pis_.nextStart()) { Motion *motion = new Motion(siC_); si_->copyState(motion->state_, st); siC_->nullControl(motion->control_); nn_->add(motion); motion->accCost_ = opt_->identityCost(); findClosestWitness(motion); } if (nn_->size() == 0) { OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); return base::PlannerStatus::INVALID_START; } if (!sampler_) sampler_ = si_->allocStateSampler(); if (!controlSampler_) controlSampler_ = siC_->allocControlSampler(); OMPL_INFORM("%s: Starting planning with %u states already in datastructure\n", getName().c_str(), nn_->size()); Motion *solution = NULL; Motion *approxsol = NULL; double approxdif = std::numeric_limits<double>::infinity(); bool sufficientlyShort = false; Motion *rmotion = new Motion(siC_); base::State *rstate = rmotion->state_; Control *rctrl = rmotion->control_; base::State *xstate = si_->allocState(); unsigned iterations = 0; while (ptc == false) { /* sample random state (with goal biasing) */ if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample()) goal_s->sampleGoal(rstate); else sampler_->sampleUniform(rstate); /* find closest state in the tree */ Motion *nmotion = selectNode(rmotion); /* sample a random control that attempts to go towards the random state, and also sample a control duration */ controlSampler_->sample(rctrl); unsigned int cd = rng_.uniformInt(siC_->getMinControlDuration(),siC_->getMaxControlDuration()); unsigned int propCd = siC_->propagateWhileValid(nmotion->state_,rctrl,cd,rstate); if (propCd == cd) { base::Cost incCost = opt_->motionCost(nmotion->state_, rstate); base::Cost cost = opt_->combineCosts(nmotion->accCost_, incCost); Witness* closestWitness = findClosestWitness(rmotion); if (closestWitness->rep_ == rmotion || opt_->isCostBetterThan(cost,closestWitness->rep_->accCost_)) { Motion* oldRep = closestWitness->rep_; /* create a motion */ Motion *motion = new Motion(siC_); motion->accCost_ = cost; si_->copyState(motion->state_, rmotion->state_); siC_->copyControl(motion->control_, rctrl); motion->steps_ = cd; motion->parent_ = nmotion; nmotion->numChildren_++; closestWitness->linkRep(motion); nn_->add(motion); double dist = 0.0; bool solv = goal->isSatisfied(motion->state_, &dist); if (solv && opt_->isCostBetterThan(motion->accCost_,prevSolutionCost_)) { approxdif = dist; solution = motion; for (unsigned int i = 0 ; i < prevSolution_.size() ; ++i) if (prevSolution_[i]) si_->freeState(prevSolution_[i]); prevSolution_.clear(); for (unsigned int i = 0 ; i < prevSolutionControls_.size() ; ++i) if (prevSolutionControls_[i]) siC_->freeControl(prevSolutionControls_[i]); prevSolutionControls_.clear(); prevSolutionSteps_.clear(); Motion* solTrav = solution; while(solTrav->parent_!=NULL) { prevSolution_.push_back(si_->cloneState(solTrav->state_) ); prevSolutionControls_.push_back(siC_->cloneControl(solTrav->control_) ); prevSolutionSteps_.push_back(solTrav->steps_ ); solTrav = solTrav->parent_; } prevSolution_.push_back(si_->cloneState(solTrav->state_) ); prevSolutionCost_ = solution->accCost_; OMPL_INFORM("Found solution with cost %.2f",solution->accCost_.value()); sufficientlyShort = opt_->isSatisfied(solution->accCost_); if (sufficientlyShort) break; } if (solution==NULL && dist < approxdif) { approxdif = dist; approxsol = motion; for (unsigned int i = 0 ; i < prevSolution_.size() ; ++i) if (prevSolution_[i]) si_->freeState(prevSolution_[i]); prevSolution_.clear(); for (unsigned int i = 0 ; i < prevSolutionControls_.size() ; ++i) if (prevSolutionControls_[i]) siC_->freeControl(prevSolutionControls_[i]); prevSolutionControls_.clear(); prevSolutionSteps_.clear(); Motion *solTrav = approxsol; while (solTrav->parent_!=NULL) { prevSolution_.push_back(si_->cloneState(solTrav->state_) ); prevSolutionControls_.push_back(siC_->cloneControl(solTrav->control_) ); prevSolutionSteps_.push_back(solTrav->steps_ ); solTrav = solTrav->parent_; } prevSolution_.push_back(si_->cloneState(solTrav->state_) ); } if (oldRep != rmotion) { oldRep->inactive_ = true; nn_->remove(oldRep); while (oldRep->inactive_ && oldRep->numChildren_==0) { if (oldRep->state_) si_->freeState(oldRep->state_); if (oldRep->control_) siC_->freeControl(oldRep->control_); oldRep->state_=NULL; oldRep->control_=NULL; oldRep->parent_->numChildren_--; Motion* oldRepParent = oldRep->parent_; delete oldRep; oldRep = oldRepParent; } } } } iterations++; } bool solved = false; bool approximate = false; if (solution == NULL) { solution = approxsol; approximate = true; } if (solution != NULL) { /* set the solution path */ PathControl *path = new PathControl(si_); for (int i = prevSolution_.size() - 1 ; i >= 1 ; --i) path->append(prevSolution_[i], prevSolutionControls_[i-1], prevSolutionSteps_[i-1] * siC_->getPropagationStepSize()); path->append(prevSolution_[0]); solved = true; pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif, getName()); } si_->freeState(xstate); if (rmotion->state_) si_->freeState(rmotion->state_); if (rmotion->control_) siC_->freeControl(rmotion->control_); delete rmotion; OMPL_INFORM("%s: Created %u states in %u iterations", getName().c_str(), nn_->size(),iterations); return base::PlannerStatus(solved, approximate); }
ompl::base::PlannerStatus ompl::control::EST::solve(const base::PlannerTerminationCondition &ptc) { checkValidity(); base::Goal *goal = pdef_->getGoal().get(); base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal); // Initializing tree with start state(s) while (const base::State *st = pis_.nextStart()) { Motion *motion = new Motion(siC_); si_->copyState(motion->state, st); siC_->nullControl(motion->control); addMotion(motion); } if (tree_.grid.size() == 0) { OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); return base::PlannerStatus::INVALID_START; } // Ensure that we have a state sampler AND a control sampler if (!sampler_) sampler_ = si_->allocValidStateSampler(); if (!controlSampler_) controlSampler_ = siC_->allocDirectedControlSampler(); OMPL_INFORM("%s: Starting with %u states", getName().c_str(), tree_.size); Motion *solution = NULL; Motion *approxsol = NULL; double approxdif = std::numeric_limits<double>::infinity(); Motion *rmotion = new Motion(siC_); bool solved = false; while (!ptc) { // Select a state to expand the tree from Motion *existing = selectMotion(); assert (existing); // sample a random state (with goal biasing) near the state selected for expansion if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample()) goal_s->sampleGoal(rmotion->state); else { if (!sampler_->sampleNear(rmotion->state, existing->state, maxDistance_)) continue; } // Extend a motion toward the state we just sampled unsigned int duration = controlSampler_->sampleTo(rmotion->control, existing->control, existing->state, rmotion->state); // If the system was propagated for a meaningful amount of time, save into the tree if (duration >= siC_->getMinControlDuration()) { // create a motion to the resulting state Motion *motion = new Motion(siC_); si_->copyState(motion->state, rmotion->state); siC_->copyControl(motion->control, rmotion->control); motion->steps = duration; motion->parent = existing; // save the state addMotion(motion); // Check if this state is the goal state, or improves the best solution so far double dist = 0.0; solved = goal->isSatisfied(motion->state, &dist); if (solved) { approxdif = dist; solution = motion; break; } if (dist < approxdif) { approxdif = dist; approxsol = motion; } } } bool approximate = false; if (solution == NULL) { solution = approxsol; approximate = true; } // Constructing the solution path if (solution != NULL) { lastGoalMotion_ = solution; std::vector<Motion*> mpath; while (solution != NULL) { mpath.push_back(solution); solution = solution->parent; } PathControl *path = new PathControl(si_); for (int i = mpath.size() - 1 ; i >= 0 ; --i) if (mpath[i]->parent) path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize()); else path->append(mpath[i]->state); solved = true; pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif); } // Cleaning up memory if (rmotion->state) si_->freeState(rmotion->state); if (rmotion->control) siC_->freeControl(rmotion->control); delete rmotion; OMPL_INFORM("%s: Created %u states in %u cells", getName().c_str(), tree_.size, tree_.grid.size()); return base::PlannerStatus(solved, approximate); }
ompl::base::PlannerStatus ompl::control::RRT::solve(const base::PlannerTerminationCondition &ptc) { checkValidity(); base::Goal *goal = pdef_->getGoal().get(); base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal); while (const base::State *st = pis_.nextStart()) { Motion *motion = new Motion(siC_); si_->copyState(motion->state, st); siC_->nullControl(motion->control); nn_->add(motion); } if (nn_->size() == 0) { OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); return base::PlannerStatus::INVALID_START; } if (!sampler_) sampler_ = si_->allocStateSampler(); if (!controlSampler_) controlSampler_ = siC_->allocDirectedControlSampler(); OMPL_INFORM("%s: Starting with %u states", getName().c_str(), nn_->size()); Motion *solution = NULL; Motion *approxsol = NULL; double approxdif = std::numeric_limits<double>::infinity(); Motion *rmotion = new Motion(siC_); base::State *rstate = rmotion->state; Control *rctrl = rmotion->control; base::State *xstate = si_->allocState(); while (ptc == false) { /* sample random state (with goal biasing) */ if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample()) goal_s->sampleGoal(rstate); else sampler_->sampleUniform(rstate); /* find closest state in the tree */ Motion *nmotion = nn_->nearest(rmotion); /* sample a random control that attempts to go towards the random state, and also sample a control duration */ unsigned int cd = controlSampler_->sampleTo(rctrl, nmotion->control, nmotion->state, rmotion->state); if (addIntermediateStates_) { // this code is contributed by Jennifer Barry std::vector<base::State *> pstates; cd = siC_->propagateWhileValid(nmotion->state, rctrl, cd, pstates, true); if (cd >= siC_->getMinControlDuration()) { Motion *lastmotion = nmotion; bool solved = false; size_t p = 0; for ( ; p < pstates.size(); ++p) { /* create a motion */ Motion *motion = new Motion(); motion->state = pstates[p]; //we need multiple copies of rctrl motion->control = siC_->allocControl(); siC_->copyControl(motion->control, rctrl); motion->steps = 1; motion->parent = lastmotion; lastmotion = motion; nn_->add(motion); double dist = 0.0; solved = goal->isSatisfied(motion->state, &dist); if (solved) { approxdif = dist; solution = motion; break; } if (dist < approxdif) { approxdif = dist; approxsol = motion; } } //free any states after we hit the goal while (++p < pstates.size()) si_->freeState(pstates[p]); if (solved) break; } else for (size_t p = 0 ; p < pstates.size(); ++p) si_->freeState(pstates[p]); } else { if (cd >= siC_->getMinControlDuration()) { /* create a motion */ Motion *motion = new Motion(siC_); si_->copyState(motion->state, rmotion->state); siC_->copyControl(motion->control, rctrl); motion->steps = cd; motion->parent = nmotion; nn_->add(motion); double dist = 0.0; bool solv = goal->isSatisfied(motion->state, &dist); if (solv) { approxdif = dist; solution = motion; break; } if (dist < approxdif) { approxdif = dist; approxsol = motion; } } } } bool solved = false; bool approximate = false; if (solution == NULL) { solution = approxsol; approximate = true; } if (solution != NULL) { lastGoalMotion_ = solution; /* construct the solution path */ std::vector<Motion*> mpath; while (solution != NULL) { mpath.push_back(solution); solution = solution->parent; } /* set the solution path */ PathControl *path = new PathControl(si_); for (int i = mpath.size() - 1 ; i >= 0 ; --i) if (mpath[i]->parent) path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize()); else path->append(mpath[i]->state); solved = true; pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif); } if (rmotion->state) si_->freeState(rmotion->state); if (rmotion->control) siC_->freeControl(rmotion->control); delete rmotion; si_->freeState(xstate); OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size()); return base::PlannerStatus(solved, approximate); }
bool ompl::control::Syclop::solve(const base::PlannerTerminationCondition& ptc) { checkValidity(); if (!graphReady_) { numMotions_ = 0; setupRegionEstimates(); setupEdgeEstimates(); graphReady_ = true; } while (const base::State* s = pis_.nextStart()) { const int region = decomp_->locateRegion(s); startRegions_.insert(region); Motion* startMotion = addRoot(s); graph_[boost::vertex(region,graph_)].motions.push_back(startMotion); ++numMotions_; updateCoverageEstimate(graph_[boost::vertex(region,graph_)], s); } if (startRegions_.empty()) { msg_.error("There are no valid start states"); return false; } //We need at least one valid goal sample so that we can find the goal region if (goalRegions_.empty()) { if (const base::State* g = pis_.nextGoal(ptc)) goalRegions_.insert(decomp_->locateRegion(g)); else { msg_.error("Unable to sample a valid goal state"); return false; } } msg_.inform("Starting with %u states", numMotions_); std::vector<Motion*> newMotions; const Motion* solution = NULL; base::Goal* goal = pdef_->getGoal().get(); double goalDist = std::numeric_limits<double>::infinity(); bool solved = false; while (!ptc() && !solved) { const int chosenStartRegion = startRegions_.sampleUniform(); int chosenGoalRegion = -1; // if we have not sampled too many goal regions already if (pis_.haveMoreGoalStates() && goalRegions_.size() < numMotions_/2) { if (const base::State* g = pis_.nextGoal()) { chosenGoalRegion = decomp_->locateRegion(g); goalRegions_.insert(chosenGoalRegion); } } if (chosenGoalRegion == -1) chosenGoalRegion = goalRegions_.sampleUniform(); computeLead(chosenStartRegion, chosenGoalRegion); computeAvailableRegions(); for (int i = 0; i < numRegionExpansions_ && !solved && !ptc(); ++i) { const int region = selectRegion(); bool improved = false; for (int j = 0; j < numTreeSelections_ && !solved && !ptc(); ++j) { newMotions.clear(); selectAndExtend(graph_[boost::vertex(region,graph_)], newMotions); for (std::vector<Motion*>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc(); ++m) { Motion* motion = *m; double distance; solved = goal->isSatisfied(motion->state, &distance); if (solved) { goalDist = distance; solution = motion; break; } // Check for approximate (best-so-far) solution if (distance < goalDist) { goalDist = distance; solution = motion; } const int newRegion = decomp_->locateRegion(motion->state); graph_[boost::vertex(newRegion,graph_)].motions.push_back(motion); ++numMotions_; Region& newRegionObj = graph_[boost::vertex(newRegion, graph_)]; improved |= updateCoverageEstimate(newRegionObj, motion->state); if (newRegion != region) { // If this is the first time the tree has entered this region, add the region to avail if (newRegionObj.motions.size() == 1) availDist_.add(newRegion, newRegionObj.weight); /* If the tree crosses an entire region and creates an edge (u,v) for which Proj(u) and Proj(v) are non-neighboring regions, then we do not update connection estimates. This is because Syclop's shortest-path lead computation only considers neighboring regions. */ if (regionsToEdge_.count(std::pair<int,int>(region, newRegion)) > 0) { Adjacency* adj = regionsToEdge_[std::pair<int,int>(region,newRegion)]; adj->empty = false; ++adj->numSelections; improved |= updateConnectionEstimate(graph_[boost::vertex(region,graph_)], newRegionObj, motion->state); } } } } if (!improved && rng_.uniform01() < probAbandonLeadEarly_) break; } } bool addedSolution = false; if (solution != NULL) { std::vector<const Motion*> mpath; while (solution != NULL) { mpath.push_back(solution); solution = solution->parent; } PathControl* path = new PathControl(si_); for (int i = mpath.size()-1; i >= 0; --i) if (mpath[i]->parent) path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize()); else path->append(mpath[i]->state); goal->addSolutionPath(base::PathPtr(path), !solved, goalDist); addedSolution = true; } return addedSolution; }
ompl::base::PlannerStatus ompl::control::PID::solve(const base::PlannerTerminationCondition &ptc) { checkValidity(); base::Goal *goal = pdef_->getGoal().get(); while (const base::State *st = pis_.nextStart()) { motions.push_back(new Motion(siC_)); si_->copyState(motions.back()->state, st); siC_->nullControl(motions.back()->control); } if(motions.empty()) { OMPL_ERROR("Invalid Start State"); return base::PlannerStatus::INVALID_START; } if (!sampler_) { sampler_ = si_->allocStateSampler(); } if (!controlSampler_) { controlSampler_ = siC_->allocDirectedControlSampler(); } bool solved = false; double approxdif = std::numeric_limits<double>::infinity(); while (ptc == false) { /* create a motion */ // Add PID control action Motion* new_motion = new Motion(siC_); new_motion->state = si_->allocState(); siC_->nullControl(new_motion->control); control_action(new_motion->control, motions.back()->state); // Add State Propagation unsigned int cd = siC_->propagateWhileValid(motions.back()->state, new_motion->control, siC_->getMinControlDuration(), new_motion->state); motions.push_back(new_motion); double dist = 0.0; bool solv = goal->isSatisfied(new_motion->state, &dist); if(solv) { solved = true; } if(dist < approxdif) { approxdif = dist; } } /* set the solution path */ PathControl *path = new PathControl(si_); for (unsigned i = 0 ; i < motions.size()-1; ++i) { path->append(motions[i]->state, motions[i]->control, siC_->getPropagationStepSize()); } path->append(motions[motions.size()-1]->state); pdef_->addSolutionPath(base::PathPtr(path), false, approxdif); return base::PlannerStatus(true, solved); }
bool ompl::control::KPIECE1::solve(const base::PlannerTerminationCondition &ptc) { checkValidity(); base::Goal *goal = pdef_->getGoal().get(); while (const base::State *st = pis_.nextStart()) { Motion *motion = new Motion(siC_); si_->copyState(motion->state, st); siC_->nullControl(motion->control); addMotion(motion, 1.0); } if (tree_.grid.size() == 0) { msg_.error("There are no valid initial states!"); return false; } if (!controlSampler_) controlSampler_ = siC_->allocControlSampler(); msg_.inform("Starting with %u states", tree_.size); Motion *solution = NULL; Motion *approxsol = NULL; double approxdif = std::numeric_limits<double>::infinity(); Control *rctrl = siC_->allocControl(); std::vector<base::State*> states(siC_->getMaxControlDuration() + 1); std::vector<Grid::Coord> coords(states.size()); std::vector<Grid::Cell*> cells(coords.size()); for (unsigned int i = 0 ; i < states.size() ; ++i) states[i] = si_->allocState(); // samples that were found to be the best, so far CloseSamples closeSamples(nCloseSamples_); while (ptc() == false) { tree_.iteration++; /* Decide on a state to expand from */ Motion *existing = NULL; Grid::Cell *ecell = NULL; if (closeSamples.canSample() && rng_.uniform01() < goalBias_) { if (!closeSamples.selectMotion(existing, ecell)) selectMotion(existing, ecell); } else selectMotion(existing, ecell); assert(existing); /* sample a random control */ controlSampler_->sampleNext(rctrl, existing->control, existing->state); /* propagate */ unsigned int cd = controlSampler_->sampleStepCount(siC_->getMinControlDuration(), siC_->getMaxControlDuration()); cd = siC_->propagateWhileValid(existing->state, rctrl, cd, states, false); /* if we have enough steps */ if (cd >= siC_->getMinControlDuration()) { std::size_t avgCov_two_thirds = (2 * tree_.size) / (3 * tree_.grid.size()); bool interestingMotion = false; // split the motion into smaller ones, so we do not cross cell boundaries for (unsigned int i = 0 ; i < cd ; ++i) { projectionEvaluator_->computeCoordinates(states[i], coords[i]); cells[i] = tree_.grid.getCell(coords[i]); if (!cells[i]) interestingMotion = true; else { if (!interestingMotion && cells[i]->data->motions.size() <= avgCov_two_thirds) interestingMotion = true; } } if (interestingMotion || rng_.uniform01() < 0.05) { unsigned int index = 0; while (index < cd) { unsigned int nextIndex = findNextMotion(coords, index, cd); Motion *motion = new Motion(siC_); si_->copyState(motion->state, states[nextIndex]); siC_->copyControl(motion->control, rctrl); motion->steps = nextIndex - index + 1; motion->parent = existing; double dist = 0.0; bool solv = goal->isSatisfied(motion->state, &dist); Grid::Cell *toCell = addMotion(motion, dist); if (solv) { approxdif = dist; solution = motion; break; } if (dist < approxdif) { approxdif = dist; approxsol = motion; } closeSamples.consider(toCell, motion, dist); // new parent will be the newly created motion existing = motion; index = nextIndex + 1; } if (solution) break; } // update cell score ecell->data->score *= goodScoreFactor_; } else ecell->data->score *= badScoreFactor_; tree_.grid.update(ecell); } bool solved = false; bool approximate = false; if (solution == NULL) { solution = approxsol; approximate = true; } if (solution != NULL) { /* construct the solution path */ std::vector<Motion*> mpath; while (solution != NULL) { mpath.push_back(solution); solution = solution->parent; } /* set the solution path */ PathControl *path = new PathControl(si_); for (int i = mpath.size() - 1 ; i >= 0 ; --i) if (mpath[i]->parent) path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize()); else path->append(mpath[i]->state); goal->addSolutionPath(base::PathPtr(path), approximate, approxdif); solved = true; } siC_->freeControl(rctrl); for (unsigned int i = 0 ; i < states.size() ; ++i) si_->freeState(states[i]); msg_.inform("Created %u states in %u cells (%u internal + %u external)", tree_.size, tree_.grid.size(), tree_.grid.countInternal(), tree_.grid.countExternal()); return solved; }
ompl::base::PlannerStatus ompl::control::Syclop::solve(const base::PlannerTerminationCondition &ptc) { checkValidity(); if (!graphReady_) { numMotions_ = 0; setupRegionEstimates(); setupEdgeEstimates(); graphReady_ = true; } while (const base::State *s = pis_.nextStart()) { const int region = decomp_->locateRegion(s); startRegions_.insert(region); Motion *startMotion = addRoot(s); graph_[boost::vertex(region, graph_)].motions.push_back(startMotion); ++numMotions_; updateCoverageEstimate(graph_[boost::vertex(region, graph_)], s); } if (startRegions_.empty()) { OMPL_ERROR("%s: There are no valid start states", getName().c_str()); return base::PlannerStatus::INVALID_START; } //We need at least one valid goal sample so that we can find the goal region if (goalRegions_.empty()) { if (const base::State *g = pis_.nextGoal(ptc)) goalRegions_.insert(decomp_->locateRegion(g)); else { OMPL_ERROR("%s: Unable to sample a valid goal state", getName().c_str()); return base::PlannerStatus::INVALID_GOAL; } } OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), numMotions_); std::vector<Motion*> newMotions; const Motion *solution = NULL; base::Goal *goal = pdef_->getGoal().get(); double goalDist = std::numeric_limits<double>::infinity(); bool solved = false; while (!ptc && !solved) { const int chosenStartRegion = startRegions_.sampleUniform(); int chosenGoalRegion = -1; // if we have not sampled too many goal regions already if (pis_.haveMoreGoalStates() && goalRegions_.size() < numMotions_/2) { if (const base::State *g = pis_.nextGoal()) { chosenGoalRegion = decomp_->locateRegion(g); goalRegions_.insert(chosenGoalRegion); } } if (chosenGoalRegion == -1) chosenGoalRegion = goalRegions_.sampleUniform(); leadComputeFn(chosenStartRegion, chosenGoalRegion, lead_); computeAvailableRegions(); for (int i = 0; i < numRegionExpansions_ && !solved && !ptc; ++i) { const int region = selectRegion(); bool improved = false; for (int j = 0; j < numTreeSelections_ && !solved && !ptc; ++j) { newMotions.clear(); selectAndExtend(graph_[boost::vertex(region, graph_)], newMotions); for (std::vector<Motion*>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc; ++m) { Motion *motion = *m; double distance; solved = goal->isSatisfied(motion->state, &distance); if (solved) { goalDist = distance; solution = motion; break; } // Check for approximate (best-so-far) solution if (distance < goalDist) { goalDist = distance; solution = motion; } const int newRegion = decomp_->locateRegion(motion->state); graph_[boost::vertex(newRegion, graph_)].motions.push_back(motion); ++numMotions_; Region &newRegionObj = graph_[boost::vertex(newRegion, graph_)]; improved |= updateCoverageEstimate(newRegionObj, motion->state); /* If tree has just crossed from one region to its neighbor, update the connection estimates. If the tree has crossed an entire region, then region and newRegion are not adjacent, and so we do not update estimates. */ if (newRegion != region && regionsToEdge_.count(std::pair<int,int>(region,newRegion)) > 0) { Adjacency *adj = regionsToEdge_[std::pair<int,int>(region,newRegion)]; adj->empty = false; ++adj->numSelections; improved |= updateConnectionEstimate(graph_[boost::vertex(region, graph_)], newRegionObj, motion->state); } /* If this region already exists in availDist, update its weight. */ if (newRegionObj.pdfElem != NULL) availDist_.update(newRegionObj.pdfElem, newRegionObj.weight); /* Otherwise, only add this region to availDist if it already exists in the lead. */ else if (std::find(lead_.begin(), lead_.end(), newRegion) != lead_.end()) { PDF<int>::Element *elem = availDist_.add(newRegion, newRegionObj.weight); newRegionObj.pdfElem = elem; } } } if (!improved && rng_.uniform01() < probAbandonLeadEarly_) break; } } bool addedSolution = false; if (solution != NULL) { std::vector<const Motion*> mpath; while (solution != NULL) { mpath.push_back(solution); solution = solution->parent; } PathControl *path = new PathControl(si_); for (int i = mpath.size()-1; i >= 0; --i) if (mpath[i]->parent) path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize()); else path->append(mpath[i]->state); pdef_->addSolutionPath(base::PathPtr(path), !solved, goalDist, getName()); addedSolution = true; } return addedSolution ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT; }