void ompl::control::Syclop::computeLead(int startRegion, int goalRegion)
{
lead_.clear();
if (startRegion == goalRegion)
{
lead_.push_back(startRegion);
return;
}
else if (rng_.uniform01() < probShortestPath_)
{
std::vector<RegionGraph::vertex_descriptor> parents(decomp_->getNumRegions());
std::vector<double> distances(decomp_->getNumRegions());
try
{
boost::astar_search(graph_, boost::vertex(startRegion, graph_), DecompositionHeuristic(this, getRegionFromIndex(goalRegion)),
boost::weight_map(get(&Adjacency::cost, graph_)).distance_map(
boost::make_iterator_property_map(distances.begin(), get(boost::vertex_index, graph_)
)).predecessor_map(
boost::make_iterator_property_map(parents.begin(), get(boost::vertex_index, graph_))
).visitor(GoalVisitor(goalRegion))
);
}
catch (found_goal fg)
{
int region = goalRegion;
int leadLength = 1;
while (region != startRegion)
{
region = parents[region];
++leadLength;
}
lead_.resize(leadLength);
region = goalRegion;
for (int i = leadLength-1; i >= 0; --i)
{
lead_[i] = region;
region = parents[region];
}
}
}
else
{
/* Run a random-DFS over the decomposition graph from the start region to the goal region.
There may be a way to do this using boost::depth_first_search. */
VertexIndexMap index = get(boost::vertex_index, graph_);
std::stack<int> nodesToProcess;
std::vector<int> parents(decomp_->getNumRegions(), -1);
parents[startRegion] = startRegion;
nodesToProcess.push(startRegion);
bool goalFound = false;
while (!goalFound && !nodesToProcess.empty())
{
const int v = nodesToProcess.top();
nodesToProcess.pop();
std::vector<int> neighbors;
boost::graph_traits<RegionGraph>::adjacency_iterator ai, aend;
for (boost::tie(ai,aend) = adjacent_vertices(boost::vertex(v,graph_),graph_); ai != aend; ++ai)
{
if (parents[index[*ai]] < 0)
{
neighbors.push_back(index[*ai]);
parents[index[*ai]] = v;
}
}
for (std::size_t i = 0; i < neighbors.size(); ++i)
{
const int choice = rng_.uniformInt(i, neighbors.size()-1);
if (neighbors[choice] == goalRegion)
{
int region = goalRegion;
int leadLength = 1;
while (region != startRegion)
{
region = parents[region];
++leadLength;
}
lead_.resize(leadLength);
region = goalRegion;
for (int j = leadLength-1; j >= 0; --j)
{
lead_[j] = region;
region = parents[region];
}
goalFound = true;
break;
}
nodesToProcess.push(neighbors[choice]);
std::swap(neighbors[i], neighbors[choice]);
}
}
}
//Now that we have a lead, update the edge weights.
for (std::size_t i = 0; i < lead_.size()-1; ++i)
{
Adjacency& adj = *regionsToEdge_[std::pair<int,int>(lead_[i], lead_[i+1])];
if (adj.empty)
{
++adj.numLeadInclusions;
updateEdge(adj);
}
}
}
void ompl::control::SyclopRRT::selectAndExtend(Region ®ion, std::vector<Motion *> &newMotions)
{
auto *rmotion = new Motion(siC_);
base::StateSamplerPtr sampler(si_->allocStateSampler());
std::vector<double> coord(decomp_->getDimension());
decomp_->sampleFromRegion(region.index, rng_, coord);
decomp_->sampleFullState(sampler, coord, rmotion->state);
Motion *nmotion;
if (regionalNN_)
{
/* Instead of querying the nearest neighbors datastructure over the entire tree of motions,
* here we perform a linear search over all motions in the selected region and its neighbors. */
std::vector<int> searchRegions;
decomp_->getNeighbors(region.index, searchRegions);
searchRegions.push_back(region.index);
std::vector<Motion *> motions;
for (const auto &i : searchRegions)
{
const std::vector<Motion *> ®ionMotions = getRegionFromIndex(i).motions;
motions.insert(motions.end(), regionMotions.begin(), regionMotions.end());
}
std::vector<Motion *>::const_iterator i = motions.begin();
nmotion = *i;
double minDistance = distanceFunction(rmotion, nmotion);
++i;
while (i != motions.end())
{
Motion *m = *i;
const double dist = distanceFunction(rmotion, m);
if (dist < minDistance)
{
nmotion = m;
minDistance = dist;
}
++i;
}
}
else
{
assert(nn_);
nmotion = nn_->nearest(rmotion);
}
unsigned int duration =
controlSampler_->sampleTo(rmotion->control, nmotion->control, nmotion->state, rmotion->state);
if (duration >= siC_->getMinControlDuration())
{
rmotion->steps = duration;
rmotion->parent = nmotion;
newMotions.push_back(rmotion);
if (nn_)
nn_->add(rmotion);
lastGoalMotion_ = rmotion;
}
else
{
si_->freeState(rmotion->state);
siC_->freeControl(rmotion->control);
delete rmotion;
}
}
