37 #include "ompl/geometric/PathSimplifier.h"
38 #include "ompl/tools/config/MagicConstants.h"
62 std::vector<base::State*> &states = path.
getStates();
67 for (
unsigned int s = 0 ; s < maxSteps ; ++s)
71 unsigned int i = 2, u = 0, n1 = states.size() - 1;
74 if (si->isValid(states[i - 1]))
76 si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1);
77 si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2);
78 si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
79 if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1]))
81 if (si->distance(states[i], temp1) > minChange)
83 si->copyState(states[i], temp1);
108 if (maxEmptySteps == 0)
112 unsigned int nochange = 0;
114 std::vector<base::State*> &states = path.
getStates();
116 if (si->checkMotion(states.front(), states.back()))
119 for (std::size_t i = 2 ; i < states.size() ; ++i)
120 si->freeState(states[i-1]);
121 std::vector<base::State*> newStates(2);
122 newStates[0] = states.front();
123 newStates[1] = states.back();
124 states.swap(newStates);
128 for (
unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
130 int count = states.size();
131 int maxN = count - 1;
132 int range = 1 + (int)(floor(0.5 + (
double)count * rangeRatio));
134 int p1 = rng_.uniformInt(0, maxN);
135 int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
136 if (abs(p1 - p2) < 2)
150 if (si->checkMotion(states[p1], states[p2]))
153 for (
int j = p1 + 1 ; j < p2 ; ++j)
154 si->freeState(states[j]);
155 states.erase(states.begin() + p1 + 1, states.begin() + p2);
171 if (maxEmptySteps == 0)
175 std::vector<base::State*> &states = path.
getStates();
177 std::vector<double> dists(states.size(), 0.0);
178 for (
unsigned int i = 1 ; i < dists.size() ; ++i)
179 dists[i] = dists[i - 1] + si->distance(states[i-1], states[i]);
180 double threshold = dists.back() * snapToVertex;
181 double rd = rangeRatio * dists.back();
186 unsigned int nochange = 0;
187 for (
unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
192 double p0 = rng_.uniformReal(0.0, dists.back());
193 std::vector<double>::iterator pit = std::lower_bound(dists.begin(), dists.end(), p0);
194 int pos0 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
196 if (pos0 == 0 || dists[pos0] - p0 < threshold)
200 while (pos0 > 0 && p0 < dists[pos0])
202 if (p0 - dists[pos0] < threshold)
209 double p1 = rng_.uniformReal(std::max(0.0, p0 - rd), std::min(p0 + rd, dists.back()));
210 pit = std::lower_bound(dists.begin(), dists.end(), p1);
211 int pos1 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
213 if (pos1 == 0 || dists[pos1] - p1 < threshold)
217 while (pos1 > 0 && p1 < dists[pos1])
219 if (p1 - dists[pos1] < threshold)
223 if (pos0 == pos1 || index0 == pos1 || index1 == pos0 ||
224 pos0 + 1 == index1 || pos1 + 1 == index0 ||
225 (index0 >=0 && index1 >= 0 && abs(index0 - index1) < 2))
232 t0 = (p0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
233 si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
241 t1 = (p1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
242 si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
246 if (si->checkMotion(s0, s1))
250 std::swap(pos0, pos1);
251 std::swap(index0, index1);
256 if (index0 < 0 && index1 < 0)
258 if (pos0 + 1 == pos1)
260 si->copyState(states[pos1], s0);
261 states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
266 for (
int j = pos0 + 2 ; j < pos1 ; ++j)
267 si->freeState(states[j]);
268 si->copyState(states[pos0 + 1], s0);
269 si->copyState(states[pos1], s1);
270 states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
274 if (index0 >= 0 && index1 >= 0)
277 for (
int j = index0 + 1 ; j < index1 ; ++j)
278 si->freeState(states[j]);
279 states.erase(states.begin() + index0 + 1, states.begin() + index1);
282 if (index0 < 0 && index1 >= 0)
285 for (
int j = pos0 + 2 ; j < index1 ; ++j)
286 si->freeState(states[j]);
287 si->copyState(states[pos0 + 1], s0);
288 states.erase(states.begin() + pos0 + 2, states.begin() + index1);
291 if (index0 >= 0 && index1 < 0)
294 for (
int j = index0 + 1 ; j < pos1 ; ++j)
295 si->freeState(states[j]);
296 si->copyState(states[pos1], s1);
297 states.erase(states.begin() + index0 + 1, states.begin() + pos1);
301 dists.resize(states.size(), 0.0);
302 for (
unsigned int j = pos0 + 1 ; j < dists.size() ; ++j)
303 dists[j] = dists[j - 1] + si->distance(states[j-1], states[j]);
304 threshold = dists.back() * snapToVertex;
305 rd = rangeRatio * dists.back();
311 si->freeState(temp1);
312 si->freeState(temp0);
324 if (maxEmptySteps == 0)
328 std::vector<base::State*> &states = path.
getStates();
331 std::map<std::pair<const base::State*, const base::State*>,
double> distances;
332 for (
unsigned int i = 0 ; i < states.size() ; ++i)
333 for (
unsigned int j = i + 2 ; j < states.size() ; ++j)
334 distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
337 unsigned int nochange = 0;
338 for (
unsigned int s = 0 ; s < maxSteps && nochange < maxEmptySteps ; ++s, ++nochange)
341 double minDist = std::numeric_limits<double>::infinity();
344 for (
unsigned int i = 0 ; i < states.size() ; ++i)
345 for (
unsigned int j = i + 2 ; j < states.size() ; ++j)
347 double d = distances[std::make_pair(states[i], states[j])];
356 if (p1 >= 0 && p2 >= 0)
358 if (si->checkMotion(states[p1], states[p2]))
361 for (
int i = p1 + 1 ; i < p2 ; ++i)
362 si->freeState(states[i]);
363 states.erase(states.begin() + p1 + 1, states.begin() + p2);
368 distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
378 reduceVertices(path);
379 collapseCloseVertices(path);
381 if (si_->getStateSpace()->isMetricSpace())
383 smoothBSpline(path, 4, path.
length()/100.0);
386 OMPL_WARN(
"Solution path may slightly touch on an invalid region of the state space");
389 OMPL_DEBUG(
"The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
404 bool tryMore =
false;
406 tryMore = reduceVertices(path);
410 collapseCloseVertices(path);
414 while (tryMore && ptc ==
false && ++times <= 5)
415 tryMore = reduceVertices(path);
418 if(si_->getStateSpace()->isMetricSpace())
422 tryMore = shortcutPath(path);
428 while (tryMore && ptc ==
false && ++times <= 5)
429 tryMore = shortcutPath(path);
433 smoothBSpline(path, 3, path.
length()/100.0);
438 OMPL_WARN(
"Solution path may slightly touch on an invalid region of the state space");
441 OMPL_DEBUG(
"The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
bool shortcutPath(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double rangeRatio=0.33, double snapToVertex=0.005)
Given a path, attempt to shorten it while maintaining its validity. This is an iterative process that...
void simplify(PathGeometric &path, double maxTime)
Run simplification algorithms on the path for at most maxTime seconds.
void simplifyMax(PathGeometric &path)
Given a path, attempt to remove vertices from it while keeping the path valid. Then, try to smooth the path. This function applies the same set of default operations to the path, except in non-metric spaces, with the intention of simplifying it. In non-metric spaces, some operations are skipped because they do not work correctly when the triangle inequality may not hold.
const SpaceInformationPtr & getSpaceInformation(void) const
Get the space information associated to this class.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
std::vector< base::State * > & getStates(void)
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
std::size_t getStateCount(void) const
Get the number of states (way-points) that make up this path.
std::pair< bool, bool > checkAndRepair(unsigned int attempts)
Check if the path is valid. If it is not, attempts are made to fix the path by sampling around invali...
void subdivide(void)
Add a state at the middle of each segment.
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time) ...
bool reduceVertices(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double rangeRatio=0.33)
Given a path, attempt to remove vertices from it while keeping the path valid. This is an iterative p...
Definition of an abstract state.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
bool freeStates() const
Return true if the memory of states is freed when they are removed from a path during simplification...
Definition of a geometric path.
bool collapseCloseVertices(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0)
Given a path, attempt to remove vertices from it while keeping the path valid. This is an iterative p...
bool freeStates_
Flag indicating whether the states removed from a motion should be freed.
virtual double length(void) const
Compute the length of a geometric path (sum of lengths of segments that make up the path) ...
void smoothBSpline(PathGeometric &path, unsigned int maxSteps=5, double minChange=std::numeric_limits< double >::epsilon())
Given a path, attempt to smooth it (the validity of the path is maintained).
static const unsigned int MAX_VALID_SAMPLE_ATTEMPTS
When multiple attempts are needed to generate valid samples, this value defines the default number of...