37 #include "ompl/geometric/PathSimplifier.h"
38 #include "ompl/tools/config/MagicConstants.h"
39 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
40 #include "ompl/base/StateSampler.h"
49 base::OptimizationObjectivePtr obj)
50 : si_(std::move(si)), freeStates_(true)
54 gsr_ = std::dynamic_pointer_cast<base::GoalSampleableRegion>(goal);
56 OMPL_WARN(
"%s: Goal could not be cast to GoalSampleableRegion. Goal simplification will not be performed.",
65 obj_ = std::make_shared<base::PathLengthOptimizationObjective>(
si_);
82 if (path.getStateCount() < 3)
85 const base::SpaceInformationPtr &si = path.getSpaceInformation();
86 std::vector<base::State *> &states = path.getStates();
91 for (
unsigned int s = 0; s < maxSteps; ++s)
95 unsigned int i = 2, u = 0, n1 = states.size() - 1;
98 if (si->isValid(states[i - 1]))
100 si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1);
101 si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2);
102 si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
103 if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1]))
105 if (si->distance(states[i], temp1) > minChange)
107 si->copyState(states[i], temp1);
120 si->freeState(temp1);
121 si->freeState(temp2);
125 unsigned int maxEmptySteps,
double rangeRatio)
127 if (path.getStateCount() < 3)
131 maxSteps = path.getStateCount();
133 if (maxEmptySteps == 0)
134 maxEmptySteps = path.getStateCount();
137 unsigned int nochange = 0;
138 const base::SpaceInformationPtr &si = path.getSpaceInformation();
139 std::vector<base::State *> &states = path.getStates();
141 if (si->checkMotion(states.front(), states.back()))
144 for (std::size_t i = 2; i < states.size(); ++i)
145 si->freeState(states[i - 1]);
146 std::vector<base::State *> newStates(2);
147 newStates[0] = states.front();
148 newStates[1] = states.back();
149 states.swap(newStates);
153 for (
unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; ++i, ++nochange)
155 int count = states.size();
156 int maxN = count - 1;
157 int range = 1 + (int)(floor(0.5 + (
double)count * rangeRatio));
159 int p1 = rng_.uniformInt(0, maxN);
160 int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
161 if (abs(p1 - p2) < 2)
174 if (si->checkMotion(states[p1], states[p2]))
177 for (
int j = p1 + 1; j < p2; ++j)
178 si->freeState(states[j]);
179 states.erase(states.begin() + p1 + 1, states.begin() + p2);
188 unsigned int maxEmptySteps,
double rangeRatio,
double snapToVertex)
190 if (path.getStateCount() < 3)
194 maxSteps = path.getStateCount();
196 if (maxEmptySteps == 0)
197 maxEmptySteps = path.getStateCount();
199 const base::SpaceInformationPtr &si = path.getSpaceInformation();
200 std::vector<base::State *> &states = path.getStates();
203 std::vector<base::Cost> costs(states.size(), obj_->identityCost());
204 std::vector<double> dists(states.size(), 0.0);
205 for (
unsigned int i = 1; i < costs.size(); ++i)
207 costs[i] = obj_->combineCosts(costs[i - 1], obj_->motionCost(states[i - 1], states[i]));
208 dists[i] = dists[i - 1] + si->distance(states[i - 1], states[i]);
212 double threshold = dists.back() * snapToVertex;
214 double rd = rangeRatio * dists.back();
219 unsigned int nochange = 0;
222 for (
unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; ++i, ++nochange)
228 double distTo0 = rng_.uniformReal(0.0, dists.back());
230 std::lower_bound(dists.begin(), dists.end(), distTo0);
231 int pos0 = pit == dists.end() ? dists.size() - 1 :
234 if (pos0 == 0 || dists[pos0] - distTo0 < threshold)
238 while (pos0 > 0 && distTo0 < dists[pos0])
240 if (distTo0 - dists[pos0] < threshold)
249 rng_.uniformReal(std::max(0.0, distTo0 - rd),
250 std::min(distTo0 + rd, dists.back()));
251 pit = std::lower_bound(dists.begin(), dists.end(), distTo1);
252 int pos1 = pit == dists.end() ? dists.size() - 1 :
255 if (pos1 == 0 || dists[pos1] - distTo1 < threshold)
259 while (pos1 > 0 && distTo1 < dists[pos1])
261 if (distTo1 - dists[pos1] < threshold)
266 if (pos0 == pos1 || index0 == pos1 || index1 == pos0 || pos0 + 1 == index1 || pos1 + 1 == index0 ||
267 (index0 >= 0 && index1 >= 0 && abs(index0 - index1) < 2))
277 t0 = (distTo0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
278 si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
289 t1 = (distTo1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
290 si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
295 if (si->checkMotion(s0, s1))
299 std::swap(pos0, pos1);
300 std::swap(index0, index1);
306 base::Cost s0PartialCost = (index0 >= 0) ? obj_->identityCost() : obj_->motionCost(s0, states[pos0 + 1]);
307 base::Cost s1PartialCost = (index1 >= 0) ? obj_->identityCost() : obj_->motionCost(states[pos1], s1);
309 int posTemp = pos0 + 1;
310 while (posTemp < pos1)
312 alongPath = obj_->combineCosts(alongPath, obj_->motionCost(states[posTemp], states[posTemp + 1]));
315 alongPath = obj_->combineCosts(alongPath, s1PartialCost);
316 if (obj_->isCostBetterThan(alongPath, obj_->motionCost(s0, s1)))
325 if (index0 < 0 && index1 < 0)
327 if (pos0 + 1 == pos1)
329 si->copyState(states[pos1], s0);
330 states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
335 for (
int j = pos0 + 2; j < pos1; ++j)
336 si->freeState(states[j]);
337 si->copyState(states[pos0 + 1], s0);
338 si->copyState(states[pos1], s1);
339 states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
342 else if (index0 >= 0 && index1 >= 0)
345 for (
int j = index0 + 1; j < index1; ++j)
346 si->freeState(states[j]);
347 states.erase(states.begin() + index0 + 1, states.begin() + index1);
349 else if (index0 < 0 && index1 >= 0)
352 for (
int j = pos0 + 2; j < index1; ++j)
353 si->freeState(states[j]);
354 si->copyState(states[pos0 + 1], s0);
355 states.erase(states.begin() + pos0 + 2, states.begin() + index1);
357 else if (index0 >= 0 && index1 < 0)
360 for (
int j = index0 + 1; j < pos1; ++j)
361 si->freeState(states[j]);
362 si->copyState(states[pos1], s1);
363 states.erase(states.begin() + index0 + 1, states.begin() + pos1);
367 dists.resize(states.size(), 0.0);
368 costs.resize(states.size(), obj_->identityCost());
369 for (
unsigned int j = pos0 + 1; j < costs.size(); ++j)
371 costs[j] = obj_->combineCosts(costs[j - 1], obj_->motionCost(states[j - 1], states[j]));
372 dists[j] = dists[j - 1] + si->distance(states[j - 1], states[j]);
374 threshold = dists.back() * snapToVertex;
375 rd = rangeRatio * dists.back();
381 si->freeState(temp1);
382 si->freeState(temp0);
387 unsigned int maxEmptySteps,
double snapToVertex)
390 maxSteps = path.getStateCount();
392 if (maxEmptySteps == 0)
393 maxEmptySteps = path.getStateCount();
395 const base::SpaceInformationPtr &si = path.getSpaceInformation();
396 std::vector<base::State *> &states = path.getStates();
398 std::vector<double> dists(states.size(), 0.0);
399 for (
unsigned int i = 1; i < dists.size(); i++)
400 dists[i] = dists[i - 1] + si->distance(states[i - 1], states[i]);
402 std::vector<std::tuple<double, base::Cost, unsigned int>> distCostIndices;
403 for (
unsigned int i = 0; i < states.size() - 1; i++)
404 distCostIndices.push_back(
405 std::make_tuple(si->distance(states[i], states[i + 1]), obj_->motionCost(states[i], states[i + 1]), i));
408 std::sort(distCostIndices.begin(), distCostIndices.end(),
409 [
this](std::tuple<double, base::Cost, unsigned int> a, std::tuple<double, base::Cost, unsigned int> b) {
410 return obj_->isCostBetterThan(std::get<1>(b), std::get<1>(a));
413 double threshold = dists.back() * snapToVertex;
416 unsigned int nochange = 0;
418 base::StateSamplerPtr sampler = si->allocStateSampler();
427 for (
unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; i++, nochange++)
431 double costBias = -1 * rng_.halfNormalReal(-dists.back(), 0.0, 20.0);
433 if (costBias >= dists.back())
435 z = distCostIndices.size() - 1;
436 costBias = std::get<0>(distCostIndices[z]);
441 while (costBias - std::get<0>(distCostIndices[z]) > std::numeric_limits<double>::epsilon())
443 costBias -= std::get<0>(distCostIndices[z]);
448 int pos, pos_before, pos_after;
449 double distTo = dists[std::get<2>(distCostIndices[z])] + costBias;
450 selectAlongPath(dists, states, distTo, threshold, perturb_state, pos);
453 int index_before = selectAlongPath(dists, states, distTo - stepSize / 2.0, threshold, before_state, pos_before);
454 int index_after = selectAlongPath(dists, states, distTo + stepSize / 2.0, threshold, after_state, pos_after);
456 if (index_before >= 0 && index_after >= 0 && index_before == index_after)
462 sampler->sampleUniform(new_state);
463 double dist = si->distance(perturb_state, new_state);
464 si->getStateSpace()->interpolate(perturb_state, new_state, stepSize / dist, new_state);
467 if (si->checkMotion(before_state, new_state) && si->checkMotion(new_state, after_state))
471 if (pos_before == pos_after)
473 alongPath = obj_->motionCost(before_state, after_state);
479 (index_before >= 0) ? obj_->identityCost() : obj_->motionCost(before_state, states[pos_before + 1]);
480 int posTemp = (index_before >= 0) ? index_before : pos_before + 1;
481 while (posTemp < pos_after)
483 alongPath = obj_->combineCosts(alongPath, obj_->motionCost(states[posTemp], states[posTemp + 1]));
487 (index_after >= 0) ? obj_->identityCost() : obj_->motionCost(states[pos_after], after_state);
488 alongPath = obj_->combineCosts(alongPath, afterPartialCost);
492 obj_->combineCosts(obj_->motionCost(before_state, new_state), obj_->motionCost(new_state, after_state));
493 if (obj_->isCostBetterThan(alongPath, newCost) || obj_->isCostEquivalentTo(alongPath, newCost))
500 if (index_before < 0 && index_after < 0)
502 if (pos_before == pos_after)
506 states.insert(states.begin() + pos_before + 1, si->cloneState(after_state));
507 states.insert(states.begin() + pos_before + 1, si->cloneState(new_state));
508 states.insert(states.begin() + pos_before + 1, si->cloneState(before_state));
510 else if (pos_before + 1 == pos_after)
512 si->copyState(states[pos_after], before_state);
513 states.insert(states.begin() + pos_after + 1, si->cloneState(after_state));
514 states.insert(states.begin() + pos_after + 1, si->cloneState(new_state));
516 else if (pos_before + 2 == pos_after)
518 si->copyState(states[pos_before + 1], before_state);
519 si->copyState(states[pos_after], new_state);
520 states.insert(states.begin() + pos_after + 1, si->cloneState(after_state));
524 si->copyState(states[pos_before + 1], before_state);
525 si->copyState(states[pos_before + 2], new_state);
526 si->copyState(states[pos_before + 3], after_state);
528 for (
int j = pos_before + 4; j < pos_after + 1; ++j)
529 si->freeState(states[j]);
530 states.erase(states.begin() + pos_before + 4, states.begin() + pos_after + 1);
533 else if (index_before >= 0 && index_after >= 0)
535 states.insert(states.begin() + index_before + 1, si->cloneState(new_state));
537 for (
int j = index_before + 2; j < index_after + 1; ++j)
538 si->freeState(states[j]);
539 states.erase(states.begin() + index_before + 2, states.begin() + index_after + 1);
541 else if (index_before < 0 && index_after >= 0)
543 if (index_after > pos_before + 1)
545 si->copyState(states[pos_before + 1], before_state);
546 states.insert(states.begin() + pos_before + 2, si->cloneState(new_state));
548 for (
int j = pos_before + 3; j < index_after + 1; ++j)
549 si->freeState(states[j]);
550 states.erase(states.begin() + pos_before + 3, states.begin() + index_after + 1);
554 states.insert(states.begin() + pos_before + 1, si->cloneState(new_state));
555 states.insert(states.begin() + pos_before + 1, si->cloneState(before_state));
558 else if (index_before >= 0 && index_after < 0)
560 if (pos_after > index_before)
562 si->copyState(states[pos_after], new_state);
563 states.insert(states.begin() + pos_after + 1, si->cloneState(after_state));
565 for (
int j = index_before + 1; j < pos_after; ++j)
566 si->freeState(states[j]);
567 states.erase(states.begin() + index_before + 1, states.begin() + pos_after);
571 states.insert(states.begin() + index_before + 1, si->cloneState(after_state));
572 states.insert(states.begin() + index_before + 1, si->cloneState(new_state));
577 dists.resize(states.size(), 0.0);
578 for (
unsigned int j = pos_before + 1; j < dists.size(); ++j)
580 dists[j] = dists[j - 1] + si->distance(states[j - 1], states[j]);
582 distCostIndices.clear();
583 for (
unsigned int i = 0; i < states.size() - 1; i++)
584 distCostIndices.push_back(std::make_tuple(si->distance(states[i], states[i + 1]),
585 obj_->motionCost(states[i], states[i + 1]), i));
589 distCostIndices.begin(), distCostIndices.end(),
590 [
this](std::tuple<double, base::Cost, unsigned int> a, std::tuple<double, base::Cost, unsigned int> b) {
591 return obj_->isCostBetterThan(std::get<1>(b), std::get<1>(a));
593 threshold = dists.back() * snapToVertex;
598 si->freeState(perturb_state);
599 si->freeState(before_state);
600 si->freeState(after_state);
601 si->freeState(new_state);
607 unsigned int maxEmptySteps)
609 if (path.getStateCount() < 3)
613 maxSteps = path.getStateCount();
615 if (maxEmptySteps == 0)
616 maxEmptySteps = path.getStateCount();
618 const base::SpaceInformationPtr &si = path.getSpaceInformation();
619 std::vector<base::State *> &states = path.getStates();
622 std::map<std::pair<const base::State *, const base::State *>,
double> distances;
623 for (
unsigned int i = 0; i < states.size(); ++i)
624 for (
unsigned int j = i + 2; j < states.size(); ++j)
625 distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
628 unsigned int nochange = 0;
629 for (
unsigned int s = 0; s < maxSteps && nochange < maxEmptySteps; ++s, ++nochange)
632 double minDist = std::numeric_limits<double>::infinity();
635 for (
unsigned int i = 0; i < states.size(); ++i)
636 for (
unsigned int j = i + 2; j < states.size(); ++j)
638 double d = distances[std::make_pair(states[i], states[j])];
647 if (p1 >= 0 && p2 >= 0)
649 if (si->checkMotion(states[p1], states[p2]))
652 for (
int i = p1 + 1; i < p2; ++i)
653 si->freeState(states[i]);
654 states.erase(states.begin() + p1 + 1, states.begin() + p2);
659 distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
670 return simplify(path, neverTerminate);
681 if (path.getStateCount() < 3)
684 bool tryMore =
true, valid =
true;
685 while ((ptc ==
false || atLeastOnce) && tryMore)
688 if ((ptc ==
false || atLeastOnce) && si_->getStateSpace()->isMetricSpace())
690 bool metricTryMore =
true;
691 unsigned int times = 0;
694 bool shortcut = shortcutPath(path);
696 gsr_ ? findBetterGoal(path, ptc) :
false;
698 metricTryMore = shortcut || better_goal;
699 }
while ((ptc ==
false || atLeastOnce) && ++times <= 5 && metricTryMore);
702 if (ptc ==
false || atLeastOnce)
703 smoothBSpline(path, 3, path.length() / 100.0);
705 if (ptc ==
false || atLeastOnce)
712 OMPL_WARN(
"Solution path may slightly touch on an invalid region of the state space");
715 OMPL_DEBUG(
"The solution path was slightly touching on an invalid region of the state space, but "
717 "successfully fixed.");
722 if (ptc ==
false || atLeastOnce)
723 tryMore = reduceVertices(path);
726 if (ptc ==
false || atLeastOnce)
727 collapseCloseVertices(path);
730 unsigned int times = 0;
731 while ((ptc ==
false || atLeastOnce) && tryMore && ++times <= 5)
732 tryMore = reduceVertices(path);
734 if ((ptc ==
false || atLeastOnce) && si_->getStateSpace()->isMetricSpace())
741 OMPL_WARN(
"Solution path may slightly touch on an invalid region of the state space");
744 OMPL_DEBUG(
"The solution path was slightly touching on an invalid region of the state space, but it "
746 "successfully fixed.");
751 return valid || path.check();
755 double rangeRatio,
double snapToVertex)
762 unsigned int samplingAttempts,
double rangeRatio,
765 if (path.getStateCount() < 2)
770 OMPL_WARN(
"%s: No goal sampleable object to sample a better goal from.",
"PathSimplifier::findBetterGoal");
774 unsigned int maxGoals = std::min((
unsigned)10, gsr_->maxSampleCount());
775 unsigned int failedTries = 0;
776 bool betterGoal =
false;
778 const base::StateSpacePtr &ss = si_->getStateSpace();
779 std::vector<base::State *> &states = path.getStates();
782 std::vector<base::Cost> costs(states.size(), obj_->identityCost());
783 std::vector<double> dists(states.size(), 0.0);
784 for (
unsigned int i = 1; i < dists.size(); ++i)
786 costs[i] = obj_->combineCosts(costs[i - 1], obj_->motionCost(states[i - 1], states[i]));
787 dists[i] = dists[i - 1] + si_->distance(states[i - 1], states[i]);
790 OMPL_WARN(
"%s: Somehow computed negative distance!.",
"PathSimplifier::findBetterGoal");
797 double threshold = dists.back() * snapToVertex;
799 double rd = rangeRatio * dists.back();
804 while (!ptc && failedTries++ < maxGoals && !betterGoal)
806 gsr_->sampleGoal(tempGoal);
809 if (!gsr_->isStartGoalPairValid(path.getState(0), tempGoal))
812 unsigned int numSamples = 0;
813 while (!ptc && numSamples++ < samplingAttempts && !betterGoal)
816 double t = rng_.uniformReal(std::max(dists.back() - rd, 0.0),
819 auto end = std::lower_bound(dists.begin(), dists.end(), t);
821 while (start != dists.begin() && *start >= t)
824 unsigned int startIndex = start - dists.begin();
825 unsigned int endIndex = end - dists.begin();
828 if (t - (*start) < threshold)
829 endIndex = startIndex;
830 if ((*end) - t < threshold)
831 startIndex = endIndex;
836 if (startIndex == endIndex)
838 state = states[startIndex];
842 double tSeg = (t - (*start)) / (*end - *start);
843 ss->interpolate(states[startIndex], states[endIndex], tSeg, temp);
846 costToCome = obj_->combineCosts(costToCome, obj_->motionCost(states[startIndex], state));
849 base::Cost costToGo = obj_->motionCost(state, tempGoal);
850 base::Cost candidateCost = obj_->combineCosts(costToCome, costToGo);
853 if (obj_->isCostBetterThan(candidateCost, costs.back()) && si_->checkMotion(state, tempGoal))
856 if (startIndex == endIndex)
859 si_->copyState(states[startIndex], state);
861 si_->copyState(states[startIndex + 1], tempGoal);
865 for (
size_t i = startIndex + 2; i < states.size(); ++i)
866 si_->freeState(states[i]);
868 states.erase(states.begin() + startIndex + 2, states.end());
873 si_->copyState(states[endIndex], state);
874 if (endIndex == states.size() - 1)
876 path.append(tempGoal);
881 si_->copyState(states[endIndex + 1], tempGoal);
884 for (
size_t i = endIndex + 2; i < states.size(); ++i)
885 si_->freeState(states[i]);
887 states.erase(states.begin() + endIndex + 2, states.end());
892 costs.resize(states.size(), obj_->identityCost());
893 dists.resize(states.size(), 0.0);
894 for (
unsigned int j = std::max(1u, startIndex); j < dists.size(); ++j)
896 costs[j] = obj_->combineCosts(costs[j - 1], obj_->motionCost(states[j - 1], states[j]));
897 dists[j] = dists[j - 1] + si_->distance(states[j - 1], states[j]);
905 si_->freeState(temp);
906 si_->freeState(tempGoal);
911 int ompl::geometric::PathSimplifier::selectAlongPath(std::vector<double> dists, std::vector<base::State *> states,
912 double distTo,
double threshold,
base::State *select_state,
917 else if (distTo > dists.back())
918 distTo = dists.back();
921 auto pit = std::lower_bound(dists.begin(), dists.end(), distTo);
922 pos = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
924 if (pos == 0 || dists[pos] - distTo < threshold)
928 while (pos > 0 && distTo < dists[pos])
930 if (distTo - dists[pos] < threshold)
936 si_->copyState(select_state, states[index]);
941 double t = (distTo - dists[pos]) / (dists[pos + 1] - dists[pos]);
942 si_->getStateSpace()->interpolate(states[pos], states[pos + 1], t, select_state);