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::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 = nullptr;
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 != nullptr)
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 != nullptr)
{
std::vector<const Motion *> mpath;
while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
}
auto path(std::make_shared<PathControl>(si_));
for (int i = mpath.size() - 1; i >= 0; --i)
if (mpath[i]->parent != nullptr)
path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
else
path->append(mpath[i]->state);
pdef_->addSolutionPath(path, !solved, goalDist, getName());
addedSolution = true;
}
return addedSolution ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}