bool ompl::geometric::PathSimplifier::collapseCloseVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps)
{
if (path.getStateCount() < 3)
return false;
if (maxSteps == 0)
maxSteps = path.getStateCount();
if (maxEmptySteps == 0)
maxEmptySteps = path.getStateCount();
const base::SpaceInformationPtr &si = path.getSpaceInformation();
std::vector<base::State*> &states = path.getStates();
// compute pair-wise distances in path (construct only half the matrix)
std::map<std::pair<const base::State*, const base::State*>, double> distances;
for (unsigned int i = 0 ; i < states.size() ; ++i)
for (unsigned int j = i + 2 ; j < states.size() ; ++j)
distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
bool result = false;
unsigned int nochange = 0;
for (unsigned int s = 0 ; s < maxSteps && nochange < maxEmptySteps ; ++s, ++nochange)
{
// find closest pair of points
double minDist = std::numeric_limits<double>::infinity();
int p1 = -1;
int p2 = -1;
for (unsigned int i = 0 ; i < states.size() ; ++i)
for (unsigned int j = i + 2 ; j < states.size() ; ++j)
{
double d = distances[std::make_pair(states[i], states[j])];
if (d < minDist)
{
minDist = d;
p1 = i;
p2 = j;
}
}
if (p1 >= 0 && p2 >= 0)
{
if (si->checkMotion(states[p1], states[p2]))
{
if (freeStates_)
for (int i = p1 + 1 ; i < p2 ; ++i)
si->freeState(states[i]);
states.erase(states.begin() + p1 + 1, states.begin() + p2);
result = true;
nochange = 0;
}
else
distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
}
else
break;
}
return result;
}
void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc)
{
if (path.getStateCount() < 3)
return;
// try a randomized step of connecting vertices
bool tryMore = false;
if (ptc == false)
tryMore = reduceVertices(path);
// try to collapse close-by vertices
if (ptc == false)
collapseCloseVertices(path);
// try to reduce verices some more, if there is any point in doing so
int times = 0;
while (tryMore && ptc == false && ++times <= 5)
tryMore = reduceVertices(path);
// if the space is metric, we can do some additional smoothing
if(si_->getStateSpace()->isMetricSpace())
{
bool tryMore = true;
unsigned int times = 0;
do
{
bool shortcut = shortcutPath(path); // split path segments, not just vertices
bool better_goal = gsr_ ? findBetterGoal(path, ptc) : false; // Try to connect the path to a closer goal
tryMore = shortcut || better_goal;
} while(ptc == false && tryMore && ++times <= 5);
// smooth the path with BSpline interpolation
if(ptc == false)
smoothBSpline(path, 3, path.length()/100.0);
// we always run this if the metric-space algorithms were run. In non-metric spaces this does not work.
const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
if (!p.second)
OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
else
if (!p.first)
OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
}
}
/* Based on COMP450 2010 project of Yun Yu and Linda Hill (Rice University) */
void ompl::geometric::PathSimplifier::smoothBSpline(PathGeometric &path, unsigned int maxSteps, double minChange)
{
if (path.getStateCount() < 3)
return;
const base::SpaceInformationPtr &si = path.getSpaceInformation();
std::vector<base::State*> &states = path.getStates();
base::State *temp1 = si->allocState();
base::State *temp2 = si->allocState();
for (unsigned int s = 0 ; s < maxSteps ; ++s)
{
path.subdivide();
unsigned int i = 2, u = 0, n1 = states.size() - 1;
while (i < n1)
{
if (si->isValid(states[i - 1]))
{
si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1);
si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2);
si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1]))
{
if (si->distance(states[i], temp1) > minChange)
{
si->copyState(states[i], temp1);
++u;
}
}
}
i += 2;
}
if (u == 0)
break;
}
si->freeState(temp1);
si->freeState(temp2);
}
void ompl::geometric::PathHybridization::matchPaths(const PathGeometric &p, const PathGeometric &q, double gapCost,
std::vector<int> &indexP, std::vector<int> &indexQ) const
{
std::vector<std::vector<double> > C(p.getStateCount());
std::vector<std::vector<char> > T(p.getStateCount());
for (std::size_t i = 0 ; i < p.getStateCount() ; ++i)
{
C[i].resize(q.getStateCount(), 0.0);
T[i].resize(q.getStateCount(), '\0');
for (std::size_t j = 0 ; j < q.getStateCount() ; ++j)
{
// as far as I can tell, there is a bug in the algorithm as presented in the paper
// so I am doing things slightly differently ...
double match = si_->distance(p.getState(i), q.getState(j)) + ((i > 0 && j > 0) ? C[i - 1][j - 1] : 0.0);
double up = gapCost + (i > 0 ? C[i - 1][j] : 0.0);
double left = gapCost + (j > 0 ? C[i][j - 1] : 0.0);
if (match <= up && match <= left)
{
C[i][j] = match;
T[i][j] = 'm';
}
else
if (up <= match && up <= left)
{
C[i][j] = up;
T[i][j] = 'u';
}
else
{
C[i][j] = left;
T[i][j] = 'l';
}
}
}
// construct the sequences with gaps (only index positions)
int m = p.getStateCount() - 1;
int n = q.getStateCount() - 1;
indexP.clear();
indexQ.clear();
indexP.reserve(std::max(n,m));
indexQ.reserve(indexP.size());
while (n >= 0 && m >= 0)
{
if (T[m][n] == 'm')
{
indexP.push_back(m);
indexQ.push_back(n);
--m; --n;
}
else
if (T[m][n] == 'u')
{
indexP.push_back(m);
indexQ.push_back(-1);
--m;
}
else
{
indexP.push_back(-1);
indexQ.push_back(n);
--n;
}
}
while (n >= 0)
{
indexP.push_back(-1);
indexQ.push_back(n);
--n;
}
while (m >= 0)
{
indexP.push_back(m);
indexQ.push_back(-1);
--m;
}
}
unsigned int ompl::geometric::PathHybridization::recordPath(const base::PathPtr &pp, bool matchAcrossGaps)
{
PathGeometric *p = dynamic_cast<PathGeometric*>(pp.get());
if (!p)
{
OMPL_ERROR("Path hybridization only works for geometric paths");
return 0;
}
if (p->getSpaceInformation() != si_)
{
OMPL_ERROR("Paths for hybridization must be from the same space information");
return 0;
}
// skip empty paths
if (p->getStateCount() == 0)
return 0;
PathInfo pi(pp);
// if this path was previously included in the hybridization, skip it
if (paths_.find(pi) != paths_.end())
return 0;
// the number of connection attempts
unsigned int nattempts = 0;
// start from virtual root
Vertex v0 = boost::add_vertex(g_);
stateProperty_[v0] = pi.states_[0];
pi.vertices_.push_back(v0);
// add all the vertices of the path, and the edges between them, to the HGraph
// also compute the path length for future use (just for computational savings)
const HGraph::edge_property_type prop0(0.0);
boost::add_edge(root_, v0, prop0, g_);
double length = 0.0;
for (std::size_t j = 1 ; j < pi.states_.size() ; ++j)
{
Vertex v1 = boost::add_vertex(g_);
stateProperty_[v1] = pi.states_[j];
double weight = si_->distance(pi.states_[j-1], pi.states_[j]);
const HGraph::edge_property_type properties(weight);
boost::add_edge(v0, v1, properties, g_);
length += weight;
pi.vertices_.push_back(v1);
v0 = v1;
}
// connect to virtual goal
boost::add_edge(v0, goal_, prop0, g_);
pi.length_ = length;
// find matches with previously added paths
for (std::set<PathInfo>::const_iterator it = paths_.begin() ; it != paths_.end() ; ++it)
{
const PathGeometric *q = static_cast<const PathGeometric*>(it->path_.get());
std::vector<int> indexP, indexQ;
matchPaths(*p, *q, (pi.length_ + it->length_) / (2.0 / magic::GAP_COST_FRACTION), indexP, indexQ);
if (matchAcrossGaps)
{
int lastP = -1;
int lastQ = -1;
int gapStartP = -1;
int gapStartQ = -1;
bool gapP = false;
bool gapQ = false;
for (std::size_t i = 0 ; i < indexP.size() ; ++i)
{
// a gap is found in p
if (indexP[i] < 0)
{
// remember this as the beginning of the gap, if needed
if (!gapP)
gapStartP = i;
// mark the fact we are now in a gap on p
gapP = true;
}
else
{
// check if a gap just ended;
// if it did, try to match the endpoint with the elements in q
if (gapP)
for (std::size_t j = gapStartP ; j < i ; ++j)
{
attemptNewEdge(pi, *it, indexP[i], indexQ[j]);
++nattempts;
}
// remember the last non-negative index in p
lastP = i;
gapP = false;
}
if (indexQ[i] < 0)
{
if (!gapQ)
gapStartQ = i;
gapQ = true;
}
else
{
if (gapQ)
for (std::size_t j = gapStartQ ; j < i ; ++j)
{
attemptNewEdge(pi, *it, indexP[j], indexQ[i]);
++nattempts;
}
lastQ = i;
gapQ = false;
}
// try to match corresponding index values and gep beginnings
if (lastP >= 0 && lastQ >= 0)
{
attemptNewEdge(pi, *it, indexP[lastP], indexQ[lastQ]);
++nattempts;
}
}
}
else
{
// attempt new edge only when states align
for (std::size_t i = 0 ; i < indexP.size() ; ++i)
if (indexP[i] >= 0 && indexQ[i] >= 0)
{
attemptNewEdge(pi, *it, indexP[i], indexQ[i]);
++nattempts;
}
}
}
// remember this path is part of the hybridization
paths_.insert(pi);
return nattempts;
}
bool ompl::geometric::PathSimplifier::findBetterGoal(PathGeometric &path, const base::PlannerTerminationCondition &ptc,
unsigned int samplingAttempts, double rangeRatio, double snapToVertex)
{
if (path.getStateCount() < 2)
return false;
if (!gsr_)
{
OMPL_WARN("%s: No goal sampleable object to sample a better goal from.", "PathSimplifier::findBetterGoal");
return false;
}
unsigned int maxGoals = std::min((unsigned)10, gsr_->maxSampleCount()); // the number of goals we will sample
unsigned int failedTries = 0;
bool betterGoal = false;
const base::StateSpacePtr& ss = si_->getStateSpace();
std::vector<base::State*> &states = path.getStates();
// dists[i] contains the cumulative length of the path up to and including state i
std::vector<double> dists(states.size(), 0.0);
for (unsigned int i = 1 ; i < dists.size() ; ++i)
dists[i] = dists[i-1] + si_->distance(states[i-1], states[i]);
// Sampled states closer than 'threshold' distance to any existing state in the path
// are snapped to the close state
double threshold = dists.back() * snapToVertex;
// The range (distance) of a single connection that will be attempted
double rd = rangeRatio * dists.back();
base::State* temp = si_->allocState();
base::State* tempGoal = si_->allocState();
while(!ptc && failedTries++ < maxGoals && !betterGoal)
{
gsr_->sampleGoal(tempGoal);
// Goal state is not compatible with the start state
if (!gsr_->isStartGoalPairValid(path.getState(0), tempGoal))
continue;
unsigned int numSamples = 0;
while (!ptc && numSamples++ < samplingAttempts && !betterGoal)
{
// sample a state within rangeRatio
double t = rng_.uniformReal(std::max(dists.back() - rd, 0.0), dists.back()); // Sample a random point within rd of the end of the path
std::vector<double>::iterator end = std::lower_bound(dists.begin(), dists.end(), t);
std::vector<double>::iterator start = end;
while(start != dists.begin() && *start >= t)
start -= 1;
unsigned int startIndex = start - dists.begin();
unsigned int endIndex = end - dists.begin();
// Snap the random point to the nearest vertex, if within the threshold
if (t - (*start) < threshold) // snap to the starting waypoint
endIndex = startIndex;
if ((*end) - t < threshold) // snap to the ending waypoint
startIndex = endIndex;
// Compute the state value and the accumulated cost up to that state
double costToCome = dists[startIndex];
base::State* state;
if (startIndex == endIndex)
{
state = states[startIndex];
}
else
{
double tSeg = (t - (*start)) / (*end - *start);
ss->interpolate(states[startIndex], states[endIndex], tSeg, temp);
state = temp;
costToCome += si_->distance(states[startIndex], state);
}
double costToGo = si_->distance(state, tempGoal);
double candidateCost = costToCome + costToGo;
// Make sure we improve before attempting validation
if (dists.back() - candidateCost > std::numeric_limits<float>::epsilon() && si_->checkMotion(state, tempGoal))
{
// insert the new states
if (startIndex == endIndex)
{
// new intermediate state
si_->copyState(states[startIndex], state);
// new goal state
si_->copyState(states[startIndex+1], tempGoal);
if (freeStates_)
{
for(size_t i = startIndex + 2; i < states.size(); ++i)
si_->freeState(states[i]);
}
states.erase(states.begin() + startIndex + 2, states.end());
}
else
{
// overwriting the end of the segment with the new state
si_->copyState(states[endIndex], state);
if (endIndex == states.size()-1)
{
path.append(tempGoal);
}
else
{
// adding goal new goal state
si_->copyState(states[endIndex + 1], tempGoal);
if (freeStates_)
{
for(size_t i = endIndex + 2; i < states.size(); ++i)
si_->freeState(states[i]);
}
states.erase(states.begin() + endIndex + 2, states.end());
}
}
// fix the helper variables
dists.resize(states.size(), 0.0);
for (unsigned int j = std::max(1u, startIndex); j < dists.size() ; ++j)
dists[j] = dists[j-1] + si_->distance(states[j-1], states[j]);
betterGoal = true;
}
}
}
si_->freeState(temp);
si_->freeState(tempGoal);
return betterGoal;
}
bool ompl::geometric::PathSimplifier::shortcutPath(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio, double snapToVertex)
{
if (path.getStateCount() < 3)
return false;
if (maxSteps == 0)
maxSteps = path.getStateCount();
if (maxEmptySteps == 0)
maxEmptySteps = path.getStateCount();
const base::SpaceInformationPtr &si = path.getSpaceInformation();
std::vector<base::State*> &states = path.getStates();
// dists[i] contains the cumulative length of the path up to and including state i
std::vector<double> dists(states.size(), 0.0);
for (unsigned int i = 1 ; i < dists.size() ; ++i)
dists[i] = dists[i - 1] + si->distance(states[i-1], states[i]);
// Sampled states closer than 'threshold' distance to any existing state in the path
// are snapped to the close state
double threshold = dists.back() * snapToVertex;
// The range (distance) of a single connection that will be attempted
double rd = rangeRatio * dists.back();
base::State *temp0 = si->allocState();
base::State *temp1 = si->allocState();
bool result = false;
unsigned int nochange = 0;
// Attempt shortcutting maxSteps times or when no improvement is found after
// maxEmptySteps attempts, whichever comes first
for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
{
// Sample a random point anywhere along the path
base::State *s0 = nullptr;
int index0 = -1;
double t0 = 0.0;
double p0 = rng_.uniformReal(0.0, dists.back()); // sample a random point (p0) along the path
std::vector<double>::iterator pit = std::lower_bound(dists.begin(), dists.end(), p0); // find the NEXT waypoint after the random point
int pos0 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin(); // get the index of the NEXT waypoint after the point
if (pos0 == 0 || dists[pos0] - p0 < threshold) // snap to the NEXT waypoint
index0 = pos0;
else
{
while (pos0 > 0 && p0 < dists[pos0])
--pos0;
if (p0 - dists[pos0] < threshold) // snap to the PREVIOUS waypoint
index0 = pos0;
}
// Sample a random point within rd distance of the previously sampled point
base::State *s1 = nullptr;
int index1 = -1;
double t1 = 0.0;
double p1 = rng_.uniformReal(std::max(0.0, p0 - rd), std::min(p0 + rd, dists.back())); // sample a random point (p1) near p0
pit = std::lower_bound(dists.begin(), dists.end(), p1); // find the NEXT waypoint after the random point
int pos1 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin(); // get the index of the NEXT waypoint after the point
if (pos1 == 0 || dists[pos1] - p1 < threshold) // snap to the NEXT waypoint
index1 = pos1;
else
{
while (pos1 > 0 && p1 < dists[pos1])
--pos1;
if (p1 - dists[pos1] < threshold) // snap to the PREVIOUS waypoint
index1 = pos1;
}
// Don't waste time on points that are on the same path segment
if (pos0 == pos1 || index0 == pos1 || index1 == pos0 ||
pos0 + 1 == index1 || pos1 + 1 == index0 ||
(index0 >=0 && index1 >= 0 && abs(index0 - index1) < 2))
continue;
// Get the state pointer for p0
if (index0 >= 0)
s0 = states[index0];
else
{
t0 = (p0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
s0 = temp0;
}
// Get the state pointer for p1
if (index1 >= 0)
s1 = states[index1];
else
{
t1 = (p1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
s1 = temp1;
}
// Check for validity between s0 and s1
if (si->checkMotion(s0, s1))
{
if (pos0 > pos1)
{
std::swap(pos0, pos1);
std::swap(index0, index1);
std::swap(s0, s1);
std::swap(t0, t1);
}
// Modify the path with the new, shorter result
if (index0 < 0 && index1 < 0)
{
if (pos0 + 1 == pos1)
{
si->copyState(states[pos1], s0);
states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
}
else
{
if (freeStates_)
for (int j = pos0 + 2 ; j < pos1 ; ++j)
si->freeState(states[j]);
si->copyState(states[pos0 + 1], s0);
si->copyState(states[pos1], s1);
states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
}
}
else
if (index0 >= 0 && index1 >= 0)
{
if (freeStates_)
for (int j = index0 + 1 ; j < index1 ; ++j)
si->freeState(states[j]);
states.erase(states.begin() + index0 + 1, states.begin() + index1);
}
else
if (index0 < 0 && index1 >= 0)
{
if (freeStates_)
for (int j = pos0 + 2 ; j < index1 ; ++j)
si->freeState(states[j]);
si->copyState(states[pos0 + 1], s0);
states.erase(states.begin() + pos0 + 2, states.begin() + index1);
}
else
if (index0 >= 0 && index1 < 0)
{
if (freeStates_)
for (int j = index0 + 1 ; j < pos1 ; ++j)
si->freeState(states[j]);
si->copyState(states[pos1], s1);
states.erase(states.begin() + index0 + 1, states.begin() + pos1);
}
// fix the helper variables
dists.resize(states.size(), 0.0);
for (unsigned int j = pos0 + 1 ; j < dists.size() ; ++j)
dists[j] = dists[j - 1] + si->distance(states[j-1], states[j]);
threshold = dists.back() * snapToVertex;
rd = rangeRatio * dists.back();
result = true;
nochange = 0;
}
}
si->freeState(temp1);
si->freeState(temp0);
return result;
}
bool ompl::geometric::PathSimplifier::reduceVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio)
{
if (path.getStateCount() < 3)
return false;
if (maxSteps == 0)
maxSteps = path.getStateCount();
if (maxEmptySteps == 0)
maxEmptySteps = path.getStateCount();
bool result = false;
unsigned int nochange = 0;
const base::SpaceInformationPtr &si = path.getSpaceInformation();
std::vector<base::State*> &states = path.getStates();
if (si->checkMotion(states.front(), states.back()))
{
if (freeStates_)
for (std::size_t i = 2 ; i < states.size() ; ++i)
si->freeState(states[i-1]);
std::vector<base::State*> newStates(2);
newStates[0] = states.front();
newStates[1] = states.back();
states.swap(newStates);
result = true;
}
else
for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
{
int count = states.size();
int maxN = count - 1;
int range = 1 + (int)(floor(0.5 + (double)count * rangeRatio));
int p1 = rng_.uniformInt(0, maxN);
int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
if (abs(p1 - p2) < 2)
{
if (p1 < maxN - 1)
p2 = p1 + 2;
else
if (p1 > 1)
p2 = p1 - 2;
else
continue;
}
if (p1 > p2)
std::swap(p1, p2);
if (si->checkMotion(states[p1], states[p2]))
{
if (freeStates_)
for (int j = p1 + 1 ; j < p2 ; ++j)
si->freeState(states[j]);
states.erase(states.begin() + p1 + 1, states.begin() + p2);
nochange = 0;
result = true;
}
}
return result;
}
bool ompl::geometric::LightningRetrieveRepair::replan(const ompl::base::State *start, const ompl::base::State *goal,
PathGeometric &newPathSegment, const base::PlannerTerminationCondition &ptc)
{
// Reset problem definition
repairProblemDef_->clearSolutionPaths();
repairProblemDef_->clearStartStates();
repairProblemDef_->clearGoal();
// Reset planner
repairPlanner_->clear();
// Configure problem definition
repairProblemDef_->setStartAndGoalStates(start, goal);
// Configure planner
repairPlanner_->setProblemDefinition(repairProblemDef_);
// Solve
OMPL_INFORM("LightningRetrieveRepair: Preparing to repair path");
base::PlannerStatus lastStatus = base::PlannerStatus::UNKNOWN;
time::point startTime = time::now();
lastStatus = repairPlanner_->solve(ptc);
// Results
double planTime = time::seconds(time::now() - startTime);
if (!lastStatus)
{
OMPL_INFORM("LightningRetrieveRepair: No replan solution between disconnected states found after %f seconds", planTime);
return false;
}
// Check if approximate
if (repairProblemDef_->hasApproximateSolution() || repairProblemDef_->getSolutionDifference() > std::numeric_limits<double>::epsilon())
{
OMPL_INFORM("LightningRetrieveRepair: Solution is approximate, not using");
return false;
}
// Convert solution into a PathGeometric path
base::PathPtr p = repairProblemDef_->getSolutionPath();
if (!p)
{
OMPL_ERROR("LightningRetrieveRepair: Unable to get solution path from problem definition");
return false;
}
newPathSegment = static_cast<ompl::geometric::PathGeometric&>(*p);
// Smooth the result
OMPL_INFORM("LightningRetrieveRepair: Simplifying solution (smoothing)...");
time::point simplifyStart = time::now();
std::size_t numStates = newPathSegment.getStateCount();
psk_->simplify(newPathSegment, ptc);
double simplifyTime = time::seconds(time::now() - simplifyStart);
OMPL_INFORM("LightningRetrieveRepair: Path simplification took %f seconds and removed %d states",
simplifyTime, numStates - newPathSegment.getStateCount());
// Save the planner data for debugging purposes
repairPlannerDatas_.push_back(ompl::base::PlannerDataPtr( new ompl::base::PlannerData(si_) ));
repairPlanner_->getPlannerData( *repairPlannerDatas_.back() );
repairPlannerDatas_.back()->decoupleFromPlanner(); // copy states so that when planner unloads/clears we don't lose them
// Return success
OMPL_INFORM("LightningRetrieveRepair: solution found in %f seconds with %d states",
planTime, newPathSegment.getStateCount() );
return true;
}