37 #include "ompl/geometric/PathSimplifier.h" 38 #include "ompl/tools/config/MagicConstants.h" 47 : si_(
std::move(si)), freeStates_(true)
53 OMPL_WARN(
"%s: Goal could not be cast to GoalSampleableRegion. Goal simplification will not be performed.",
71 if (path.getStateCount() < 3)
75 std::vector<base::State *> &states = path.getStates();
80 for (
unsigned int s = 0; s < maxSteps; ++s)
84 unsigned int i = 2, u = 0, n1 = states.size() - 1;
87 if (si->isValid(states[i - 1]))
89 si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1);
90 si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2);
91 si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
92 if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1]))
94 if (si->distance(states[i], temp1) > minChange)
96 si->copyState(states[i], temp1);
109 si->freeState(temp1);
110 si->freeState(temp2);
114 unsigned int maxEmptySteps,
double rangeRatio)
116 if (path.getStateCount() < 3)
120 maxSteps = path.getStateCount();
122 if (maxEmptySteps == 0)
123 maxEmptySteps = path.getStateCount();
126 unsigned int nochange = 0;
128 std::vector<base::State *> &states = path.getStates();
130 if (si->checkMotion(states.front(), states.back()))
133 for (std::size_t i = 2; i < states.size(); ++i)
134 si->freeState(states[i - 1]);
135 std::vector<base::State *> newStates(2);
136 newStates[0] = states.front();
137 newStates[1] = states.back();
138 states.swap(newStates);
142 for (
unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; ++i, ++nochange)
144 int count = states.size();
145 int maxN = count - 1;
146 int range = 1 + (int)(floor(0.5 + (
double)count * rangeRatio));
148 int p1 = rng_.uniformInt(0, maxN);
149 int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
150 if (abs(p1 - p2) < 2)
163 if (si->checkMotion(states[p1], states[p2]))
166 for (
int j = p1 + 1; j < p2; ++j)
167 si->freeState(states[j]);
168 states.erase(states.begin() + p1 + 1, states.begin() + p2);
177 unsigned int maxEmptySteps,
double rangeRatio,
double snapToVertex)
179 if (path.getStateCount() < 3)
183 maxSteps = path.getStateCount();
185 if (maxEmptySteps == 0)
186 maxEmptySteps = path.getStateCount();
189 std::vector<base::State *> &states = path.getStates();
192 std::vector<double> dists(states.size(), 0.0);
193 for (
unsigned int i = 1; i < dists.size(); ++i)
194 dists[i] = dists[i - 1] + si->distance(states[i - 1], states[i]);
197 double threshold = dists.back() * snapToVertex;
199 double rd = rangeRatio * dists.back();
204 unsigned int nochange = 0;
207 for (
unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; ++i, ++nochange)
213 double p0 = rng_.uniformReal(0.0, dists.back());
214 auto pit = std::lower_bound(dists.begin(), dists.end(), p0);
215 int pos0 = pit == dists.end() ? dists.size() - 1 :
218 if (pos0 == 0 || dists[pos0] - p0 < threshold)
222 while (pos0 > 0 && p0 < dists[pos0])
224 if (p0 - dists[pos0] < threshold)
232 double p1 = rng_.uniformReal(std::max(0.0, p0 - rd),
233 std::min(p0 + rd, dists.back()));
234 pit = std::lower_bound(dists.begin(), dists.end(), p1);
235 int pos1 = pit == dists.end() ? dists.size() - 1 :
238 if (pos1 == 0 || dists[pos1] - p1 < threshold)
242 while (pos1 > 0 && p1 < dists[pos1])
244 if (p1 - dists[pos1] < threshold)
249 if (pos0 == pos1 || index0 == pos1 || index1 == pos0 || pos0 + 1 == index1 || pos1 + 1 == index0 ||
250 (index0 >= 0 && index1 >= 0 && abs(index0 - index1) < 2))
258 t0 = (p0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
259 si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
268 t1 = (p1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
269 si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
274 if (si->checkMotion(s0, s1))
278 std::swap(pos0, pos1);
279 std::swap(index0, index1);
285 if (index0 < 0 && index1 < 0)
287 if (pos0 + 1 == pos1)
289 si->copyState(states[pos1], s0);
290 states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
295 for (
int j = pos0 + 2; j < pos1; ++j)
296 si->freeState(states[j]);
297 si->copyState(states[pos0 + 1], s0);
298 si->copyState(states[pos1], s1);
299 states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
302 else if (index0 >= 0 && index1 >= 0)
305 for (
int j = index0 + 1; j < index1; ++j)
306 si->freeState(states[j]);
307 states.erase(states.begin() + index0 + 1, states.begin() + index1);
309 else if (index0 < 0 && index1 >= 0)
312 for (
int j = pos0 + 2; j < index1; ++j)
313 si->freeState(states[j]);
314 si->copyState(states[pos0 + 1], s0);
315 states.erase(states.begin() + pos0 + 2, states.begin() + index1);
317 else if (index0 >= 0 && index1 < 0)
320 for (
int j = index0 + 1; j < pos1; ++j)
321 si->freeState(states[j]);
322 si->copyState(states[pos1], s1);
323 states.erase(states.begin() + index0 + 1, states.begin() + pos1);
327 dists.resize(states.size(), 0.0);
328 for (
unsigned int j = pos0 + 1; j < dists.size(); ++j)
329 dists[j] = dists[j - 1] + si->distance(states[j - 1], states[j]);
330 threshold = dists.back() * snapToVertex;
331 rd = rangeRatio * dists.back();
337 si->freeState(temp1);
338 si->freeState(temp0);
343 unsigned int maxEmptySteps)
345 if (path.getStateCount() < 3)
349 maxSteps = path.getStateCount();
351 if (maxEmptySteps == 0)
352 maxEmptySteps = path.getStateCount();
355 std::vector<base::State *> &states = path.getStates();
358 std::map<std::pair<const base::State *, const base::State *>,
double> distances;
359 for (
unsigned int i = 0; i < states.size(); ++i)
360 for (
unsigned int j = i + 2; j < states.size(); ++j)
361 distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
364 unsigned int nochange = 0;
365 for (
unsigned int s = 0; s < maxSteps && nochange < maxEmptySteps; ++s, ++nochange)
368 double minDist = std::numeric_limits<double>::infinity();
371 for (
unsigned int i = 0; i < states.size(); ++i)
372 for (
unsigned int j = i + 2; j < states.size(); ++j)
374 double d = distances[std::make_pair(states[i], states[j])];
383 if (p1 >= 0 && p2 >= 0)
385 if (si->checkMotion(states[p1], states[p2]))
388 for (
int i = p1 + 1; i < p2; ++i)
389 si->freeState(states[i]);
390 states.erase(states.begin() + p1 + 1, states.begin() + p2);
395 distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
406 simplify(path, neverTerminate);
416 if (path.getStateCount() < 3)
420 bool tryMore =
false;
422 tryMore = reduceVertices(path);
426 collapseCloseVertices(path);
430 while (tryMore && ptc ==
false && ++times <= 5)
431 tryMore = reduceVertices(path);
434 if (si_->getStateSpace()->isMetricSpace())
437 unsigned int times = 0;
440 bool shortcut = shortcutPath(path);
441 bool better_goal = gsr_ ? findBetterGoal(path, ptc) :
false;
443 tryMore = shortcut || better_goal;
444 }
while (ptc ==
false && tryMore && ++times <= 5);
448 smoothBSpline(path, 3, path.length() / 100.0);
453 OMPL_WARN(
"Solution path may slightly touch on an invalid region of the state space");
455 OMPL_DEBUG(
"The solution path was slightly touching on an invalid region of the state space, but it was " 456 "successfully fixed.");
461 double rangeRatio,
double snapToVertex)
468 unsigned int samplingAttempts,
double rangeRatio,
471 if (path.getStateCount() < 2)
476 OMPL_WARN(
"%s: No goal sampleable object to sample a better goal from.",
"PathSimplifier::findBetterGoal");
480 unsigned int maxGoals = std::min((
unsigned)10, gsr_->maxSampleCount());
481 unsigned int failedTries = 0;
482 bool betterGoal =
false;
485 std::vector<base::State *> &states = path.getStates();
488 std::vector<double> dists(states.size(), 0.0);
489 for (
unsigned int i = 1; i < dists.size(); ++i)
490 dists[i] = dists[i - 1] + si_->distance(states[i - 1], states[i]);
494 double threshold = dists.back() * snapToVertex;
496 double rd = rangeRatio * dists.back();
501 while (!ptc && failedTries++ < maxGoals && !betterGoal)
503 gsr_->sampleGoal(tempGoal);
506 if (!gsr_->isStartGoalPairValid(path.getState(0), tempGoal))
509 unsigned int numSamples = 0;
510 while (!ptc && numSamples++ < samplingAttempts && !betterGoal)
513 double t = rng_.uniformReal(std::max(dists.back() - rd, 0.0),
516 auto end = std::lower_bound(dists.begin(), dists.end(), t);
518 while (start != dists.begin() && *start >= t)
521 unsigned int startIndex = start - dists.begin();
522 unsigned int endIndex = end - dists.begin();
525 if (t - (*start) < threshold)
526 endIndex = startIndex;
527 if ((*end) - t < threshold)
528 startIndex = endIndex;
531 double costToCome = dists[startIndex];
533 if (startIndex == endIndex)
535 state = states[startIndex];
539 double tSeg = (t - (*start)) / (*end - *start);
540 ss->interpolate(states[startIndex], states[endIndex], tSeg, temp);
543 costToCome += si_->distance(states[startIndex], state);
546 double costToGo = si_->distance(state, tempGoal);
547 double candidateCost = costToCome + costToGo;
550 if (dists.back() - candidateCost > std::numeric_limits<float>::epsilon() &&
551 si_->checkMotion(state, tempGoal))
554 if (startIndex == endIndex)
557 si_->copyState(states[startIndex], state);
559 si_->copyState(states[startIndex + 1], tempGoal);
563 for (
size_t i = startIndex + 2; i < states.size(); ++i)
564 si_->freeState(states[i]);
566 states.erase(states.begin() + startIndex + 2, states.end());
571 si_->copyState(states[endIndex], state);
572 if (endIndex == states.size() - 1)
574 path.append(tempGoal);
579 si_->copyState(states[endIndex + 1], tempGoal);
582 for (
size_t i = endIndex + 2; i < states.size(); ++i)
583 si_->freeState(states[i]);
585 states.erase(states.begin() + endIndex + 2, states.end());
590 dists.resize(states.size(), 0.0);
591 for (
unsigned int j = std::max(1u, startIndex); j < dists.size(); ++j)
592 dists[j] = dists[j - 1] + si_->distance(states[j - 1], states[j]);
599 si_->freeState(temp);
600 si_->freeState(tempGoal);
bool freeStates() const
Return true if the memory of states is freed when they are removed from a path during simplification...
bool findBetterGoal(PathGeometric &path, double maxTime, unsigned int samplingAttempts=10, double rangeRatio=0.33, double snapToVertex=0.005)
Attempt to improve the solution path by sampling a new goal state and connecting this state to the so...
PlannerTerminationCondition plannerNonTerminatingCondition()
Simple termination condition that always returns false. The termination condition will never be met...
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.
A shared pointer wrapper for ompl::base::StateSpace.
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.
PathSimplifier(base::SpaceInformationPtr si, const base::GoalPtr &goal=ompl::base::GoalPtr())
Create an instance for a specified space information. Optionally, a GoalSampleableRegion may be passe...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
std::shared_ptr< base::GoalSampleableRegion > gsr_
The goal object for the path simplifier. Used for end-of-path improvements.
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time) ...
Abstract definition of a goal region that can be sampled.
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.
A shared pointer wrapper for ompl::base::Goal.
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...
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...