Loading...
Searching...
No Matches
PathSimplifier.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Ioan Sucan, Ryan Luna */
36
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"
41#include <algorithm>
42#include <limits>
43#include <cstdlib>
44#include <cmath>
45#include <map>
46#include <utility>
47
48ompl::geometric::PathSimplifier::PathSimplifier(base::SpaceInformationPtr si, const base::GoalPtr &goal,
49 const base::OptimizationObjectivePtr& obj)
50 : si_(std::move(si)), freeStates_(true)
51{
52 if (goal)
53 {
54 gsr_ = std::dynamic_pointer_cast<base::GoalSampleableRegion>(goal);
55 if (!gsr_)
56 OMPL_WARN("%s: Goal could not be cast to GoalSampleableRegion. Goal simplification will not be performed.",
57 __FUNCTION__);
58 }
59 if (obj)
60 {
61 obj_ = obj;
62 }
63 else
64 {
65 obj_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
66 }
67}
68
70{
71 return freeStates_;
72}
73
75{
76 freeStates_ = flag;
77}
78
79/* Based on COMP450 2010 project of Yun Yu and Linda Hill (Rice University) */
80void ompl::geometric::PathSimplifier::smoothBSpline(PathGeometric &path, unsigned int maxSteps, double minChange)
81{
82 if (path.getStateCount() < 3)
83 return;
84
85 const base::SpaceInformationPtr &si = path.getSpaceInformation();
86 std::vector<base::State *> &states = path.getStates();
87
88 base::State *temp1 = si->allocState();
89 base::State *temp2 = si->allocState();
90
91 for (unsigned int s = 0; s < maxSteps; ++s)
92 {
93 path.subdivide();
94
95 unsigned int i = 2, u = 0, n1 = states.size() - 1;
96 while (i < n1)
97 {
98 if (si->isValid(states[i - 1]))
99 {
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]))
104 {
105 if (si->distance(states[i], temp1) > minChange)
106 {
107 si->copyState(states[i], temp1);
108 ++u;
109 }
110 }
111 }
112
113 i += 2;
114 }
115
116 if (u == 0)
117 break;
118 }
119
120 si->freeState(temp1);
121 si->freeState(temp2);
122}
123
125 unsigned int maxEmptySteps, double rangeRatio)
126{
127 if (path.getStateCount() < 3)
128 return false;
129
130 if (maxSteps == 0)
131 maxSteps = path.getStateCount();
132
133 if (maxEmptySteps == 0)
134 maxEmptySteps = path.getStateCount();
135
136 bool result = false;
137 unsigned int nochange = 0;
138 const base::SpaceInformationPtr &si = path.getSpaceInformation();
139 std::vector<base::State *> &states = path.getStates();
140
141 if (si->checkMotion(states.front(), states.back()))
142 {
143 if (freeStates_)
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);
150 result = true;
151 }
152 else
153 for (unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; ++i, ++nochange)
154 {
155 int count = states.size();
156 int maxN = count - 1;
157 int range = 1 + (int)(floor(0.5 + (double)count * rangeRatio));
158
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)
162 {
163 if (p1 < maxN - 1)
164 p2 = p1 + 2;
165 else if (p1 > 1)
166 p2 = p1 - 2;
167 else
168 continue;
169 }
170
171 if (p1 > p2)
172 std::swap(p1, p2);
173
174 if (si->checkMotion(states[p1], states[p2]))
175 {
176 if (freeStates_)
177 for (int j = p1 + 1; j < p2; ++j)
178 si->freeState(states[j]);
179 states.erase(states.begin() + p1 + 1, states.begin() + p2);
180 nochange = 0;
181 result = true;
182 }
183 }
184 return result;
185}
186
188 unsigned int maxEmptySteps, double rangeRatio, double snapToVertex)
189{
190 if (path.getStateCount() < 3)
191 return false;
192
193 if (maxSteps == 0)
194 maxSteps = path.getStateCount();
195
196 if (maxEmptySteps == 0)
197 maxEmptySteps = path.getStateCount();
198
199 const base::SpaceInformationPtr &si = path.getSpaceInformation();
200 std::vector<base::State *> &states = path.getStates();
201
202 // costs[i] contains the cumulative cost of the path up to and including state i
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)
206 {
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]);
209 }
210 // Sampled states closer than 'threshold' distance to any existing state in the path
211 // are snapped to the close state
212 double threshold = dists.back() * snapToVertex;
213 // The range (distance) of a single connection that will be attempted
214 double rd = rangeRatio * dists.back();
215
216 base::State *temp0 = si->allocState();
217 base::State *temp1 = si->allocState();
218 bool result = false;
219 unsigned int nochange = 0;
220 // Attempt shortcutting maxSteps times or when no improvement is found after
221 // maxEmptySteps attempts, whichever comes first
222 for (unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; ++i, ++nochange)
223 {
224 // Sample a random point anywhere along the path
225 base::State *s0 = nullptr;
226 int index0 = -1;
227 double t0 = 0.0;
228 double distTo0 = rng_.uniformReal(0.0, dists.back()); // sample a random point (p0) along the path
229 auto pit =
230 std::lower_bound(dists.begin(), dists.end(), distTo0); // find the NEXT waypoint after the random point
231 int pos0 = pit == dists.end() ? dists.size() - 1 :
232 pit - dists.begin(); // get the index of the NEXT waypoint after the point
233
234 if (pos0 == 0 || dists[pos0] - distTo0 < threshold) // snap to the NEXT waypoint
235 index0 = pos0;
236 else
237 {
238 while (pos0 > 0 && distTo0 < dists[pos0])
239 --pos0;
240 if (distTo0 - dists[pos0] < threshold) // snap to the PREVIOUS waypoint
241 index0 = pos0;
242 }
243
244 // Sample a random point within rd distance of the previously sampled point
245 base::State *s1 = nullptr;
246 int index1 = -1;
247 double t1 = 0.0;
248 double distTo1 =
249 rng_.uniformReal(std::max(0.0, distTo0 - rd),
250 std::min(distTo0 + rd, dists.back())); // sample a random point (distTo1) near s0
251 pit = std::lower_bound(dists.begin(), dists.end(), distTo1); // find the NEXT waypoint after the random point
252 int pos1 = pit == dists.end() ? dists.size() - 1 :
253 pit - dists.begin(); // get the index of the NEXT waypoint after the point
254
255 if (pos1 == 0 || dists[pos1] - distTo1 < threshold) // snap to the NEXT waypoint
256 index1 = pos1;
257 else
258 {
259 while (pos1 > 0 && distTo1 < dists[pos1])
260 --pos1;
261 if (distTo1 - dists[pos1] < threshold) // snap to the PREVIOUS waypoint
262 index1 = pos1;
263 }
264
265 // Don't waste time on points that are on the same path segment
266 if (pos0 == pos1 || index0 == pos1 || index1 == pos0 || pos0 + 1 == index1 || pos1 + 1 == index0 ||
267 (index0 >= 0 && index1 >= 0 && abs(index0 - index1) < 2))
268 continue;
269
270 // Get the state pointer for costTo0
271 if (index0 >= 0)
272 {
273 s0 = states[index0];
274 }
275 else
276 {
277 t0 = (distTo0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
278 si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
279 s0 = temp0;
280 }
281
282 // Get the state pointer for costTo1
283 if (index1 >= 0)
284 {
285 s1 = states[index1];
286 }
287 else
288 {
289 t1 = (distTo1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
290 si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
291 s1 = temp1;
292 }
293
294 // Check for validity between s0 and s1
295 if (si->checkMotion(s0, s1))
296 {
297 if (pos0 > pos1)
298 {
299 std::swap(pos0, pos1);
300 std::swap(index0, index1);
301 std::swap(s0, s1);
302 std::swap(t0, t1);
303 }
304
305 // Now that states are in the right order, make sure the cost actually decreases.
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);
308 base::Cost alongPath = s0PartialCost;
309 int posTemp = pos0 + 1;
310 while (posTemp < pos1)
311 {
312 alongPath = obj_->combineCosts(alongPath, obj_->motionCost(states[posTemp], states[posTemp + 1]));
313 posTemp++;
314 }
315 alongPath = obj_->combineCosts(alongPath, s1PartialCost);
316 if (obj_->isCostBetterThan(alongPath, obj_->motionCost(s0, s1)))
317 {
318 // The cost along the path from state 0 to 1 is better than the straight line motion cost between the
319 // two.
320 continue;
321 }
322 // Otherwise, shortcut cost is better!
323
324 // Modify the path with the new, shorter result
325 if (index0 < 0 && index1 < 0)
326 {
327 if (pos0 + 1 == pos1)
328 {
329 si->copyState(states[pos1], s0);
330 states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
331 }
332 else
333 {
334 if (freeStates_)
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);
340 }
341 }
342 else if (index0 >= 0 && index1 >= 0)
343 {
344 if (freeStates_)
345 for (int j = index0 + 1; j < index1; ++j)
346 si->freeState(states[j]);
347 states.erase(states.begin() + index0 + 1, states.begin() + index1);
348 }
349 else if (index0 < 0 && index1 >= 0)
350 {
351 if (freeStates_)
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);
356 }
357 else if (index0 >= 0 && index1 < 0)
358 {
359 if (freeStates_)
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);
364 }
365
366 // fix the helper variables
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)
370 {
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]);
373 }
374 threshold = dists.back() * snapToVertex;
375 rd = rangeRatio * dists.back();
376 result = true;
377 nochange = 0;
378 }
379 }
380
381 si->freeState(temp1);
382 si->freeState(temp0);
383 return result;
384}
385
386bool ompl::geometric::PathSimplifier::perturbPath(PathGeometric &path, double stepSize, unsigned int maxSteps,
387 unsigned int maxEmptySteps, double snapToVertex)
388{
389 if (maxSteps == 0)
390 maxSteps = path.getStateCount();
391
392 if (maxEmptySteps == 0)
393 maxEmptySteps = path.getStateCount();
394
395 const base::SpaceInformationPtr &si = path.getSpaceInformation();
396 std::vector<base::State *> &states = path.getStates();
397
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]);
401
402 std::vector<std::tuple<double, base::Cost, unsigned int>> distCostIndices;
403 for (unsigned int i = 0; i < states.size() - 1; i++)
404 distCostIndices.emplace_back(si->distance(states[i], states[i + 1]), obj_->motionCost(states[i], states[i + 1]), i);
405
406 // Sort so highest costs are first
407 std::sort(distCostIndices.begin(), distCostIndices.end(),
408 [this](std::tuple<double, base::Cost, unsigned int> a, std::tuple<double, base::Cost, unsigned int> b) {
409 return obj_->isCostBetterThan(std::get<1>(b), std::get<1>(a));
410 });
411
412 double threshold = dists.back() * snapToVertex;
413
414 bool result = false;
415 unsigned int nochange = 0;
416
417 base::StateSamplerPtr sampler = si->allocStateSampler();
418
419 base::State *perturb_state = si->allocState();
420 base::State *before_state = si->allocState();
421 base::State *after_state = si->allocState();
422 base::State *new_state = si->allocState();
423
424 // Attempt perturbing maxSteps times or when no improvement is found after
425 // maxEmptySteps attempts, whichever comes first.
426 for (unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; i++, nochange++)
427 {
428 // select a configuration on the path, biased towards high cost segments.
429 // Get a random real from 0 to the path length, biased towards 0.
430 double costBias = -1 * rng_.halfNormalReal(-dists.back(), 0.0, 20.0);
431 unsigned int z = 0;
432 if (costBias >= dists.back())
433 {
434 z = distCostIndices.size() - 1;
435 costBias = std::get<0>(distCostIndices[z]);
436 }
437 else
438 {
439 // Using our sorted cost vector, find the segment we want to use.
440 while (costBias - std::get<0>(distCostIndices[z]) > std::numeric_limits<double>::epsilon())
441 {
442 costBias -= std::get<0>(distCostIndices[z]);
443 z++;
444 }
445 }
446
447 int pos, pos_before, pos_after;
448 double distTo = dists[std::get<2>(distCostIndices[z])] + costBias;
449 selectAlongPath(dists, states, distTo, threshold, perturb_state, pos);
450
451 // Get before state and after state, that are around stepsize/2 on either side of perturb state.
452 int index_before = selectAlongPath(dists, states, distTo - stepSize / 2.0, threshold, before_state, pos_before);
453 int index_after = selectAlongPath(dists, states, distTo + stepSize / 2.0, threshold, after_state, pos_after);
454
455 if (index_before >= 0 && index_after >= 0 && index_before == index_after)
456 {
457 continue;
458 }
459
460 // Pick a random direction to extend and take a stepSize step in that direction.
461 sampler->sampleUniform(new_state);
462 double dist = si->distance(perturb_state, new_state);
463 si->getStateSpace()->interpolate(perturb_state, new_state, stepSize / dist, new_state);
464
465 // Check for validity of the new path to the new state.
466 if (si->checkMotion(before_state, new_state) && si->checkMotion(new_state, after_state))
467 {
468 // Now check for improved cost. Get the original cost along the path.
469 base::Cost alongPath;
470 if (pos_before == pos_after)
471 {
472 alongPath = obj_->motionCost(before_state, after_state);
473 }
474 else
475 {
476 // The partial cost from before_state to the first waypoint.
477 alongPath =
478 (index_before >= 0) ? obj_->identityCost() : obj_->motionCost(before_state, states[pos_before + 1]);
479 int posTemp = (index_before >= 0) ? index_before : pos_before + 1;
480 while (posTemp < pos_after)
481 {
482 alongPath = obj_->combineCosts(alongPath, obj_->motionCost(states[posTemp], states[posTemp + 1]));
483 posTemp++;
484 }
485 base::Cost afterPartialCost =
486 (index_after >= 0) ? obj_->identityCost() : obj_->motionCost(states[pos_after], after_state);
487 alongPath = obj_->combineCosts(alongPath, afterPartialCost);
488 }
489
490 base::Cost newCost =
491 obj_->combineCosts(obj_->motionCost(before_state, new_state), obj_->motionCost(new_state, after_state));
492 if (obj_->isCostBetterThan(alongPath, newCost) || obj_->isCostEquivalentTo(alongPath, newCost))
493 {
494 // Cost along the current path is better than the perturbed path.
495 continue;
496 }
497
498 // Modify the path with the new state.
499 if (index_before < 0 && index_after < 0)
500 {
501 if (pos_before == pos_after)
502 {
503 // Insert all 3 states; new_state goes before after_state, and before_state
504 // goes before new_state.
505 states.insert(states.begin() + pos_before + 1, si->cloneState(after_state));
506 states.insert(states.begin() + pos_before + 1, si->cloneState(new_state));
507 states.insert(states.begin() + pos_before + 1, si->cloneState(before_state));
508 }
509 else if (pos_before + 1 == pos_after)
510 {
511 si->copyState(states[pos_after], before_state);
512 states.insert(states.begin() + pos_after + 1, si->cloneState(after_state));
513 states.insert(states.begin() + pos_after + 1, si->cloneState(new_state));
514 }
515 else if (pos_before + 2 == pos_after)
516 {
517 si->copyState(states[pos_before + 1], before_state);
518 si->copyState(states[pos_after], new_state);
519 states.insert(states.begin() + pos_after + 1, si->cloneState(after_state));
520 }
521 else
522 {
523 si->copyState(states[pos_before + 1], before_state);
524 si->copyState(states[pos_before + 2], new_state);
525 si->copyState(states[pos_before + 3], after_state);
526 if (freeStates_)
527 for (int j = pos_before + 4; j < pos_after + 1; ++j)
528 si->freeState(states[j]);
529 states.erase(states.begin() + pos_before + 4, states.begin() + pos_after + 1);
530 }
531 }
532 else if (index_before >= 0 && index_after >= 0)
533 {
534 states.insert(states.begin() + index_before + 1, si->cloneState(new_state));
535 if (freeStates_)
536 for (int j = index_before + 2; j < index_after + 1; ++j)
537 si->freeState(states[j]);
538 states.erase(states.begin() + index_before + 2, states.begin() + index_after + 1);
539 }
540 else if (index_before < 0 && index_after >= 0)
541 {
542 if (index_after > pos_before + 1)
543 {
544 si->copyState(states[pos_before + 1], before_state);
545 states.insert(states.begin() + pos_before + 2, si->cloneState(new_state));
546 if (freeStates_)
547 for (int j = pos_before + 3; j < index_after + 1; ++j)
548 si->freeState(states[j]);
549 states.erase(states.begin() + pos_before + 3, states.begin() + index_after + 1);
550 }
551 else
552 {
553 states.insert(states.begin() + pos_before + 1, si->cloneState(new_state));
554 states.insert(states.begin() + pos_before + 1, si->cloneState(before_state));
555 }
556 }
557 else if (index_before >= 0 && index_after < 0)
558 {
559 if (pos_after > index_before)
560 {
561 si->copyState(states[pos_after], new_state);
562 states.insert(states.begin() + pos_after + 1, si->cloneState(after_state));
563 if (freeStates_)
564 for (int j = index_before + 1; j < pos_after; ++j)
565 si->freeState(states[j]);
566 states.erase(states.begin() + index_before + 1, states.begin() + pos_after);
567 }
568 else
569 {
570 states.insert(states.begin() + index_before + 1, si->cloneState(after_state));
571 states.insert(states.begin() + index_before + 1, si->cloneState(new_state));
572 }
573 }
574
575 // fix the helper variables
576 dists.resize(states.size(), 0.0);
577 for (unsigned int j = pos_before + 1; j < dists.size(); ++j)
578 {
579 dists[j] = dists[j - 1] + si->distance(states[j - 1], states[j]);
580 }
581 distCostIndices.clear();
582 for (unsigned int i = 0; i < states.size() - 1; i++)
583 distCostIndices.emplace_back(si->distance(states[i], states[i + 1]),
584 obj_->motionCost(states[i], states[i + 1]), i);
585
586 // Sort so highest costs are first
587 std::sort(
588 distCostIndices.begin(), distCostIndices.end(),
589 [this](std::tuple<double, base::Cost, unsigned int> a, std::tuple<double, base::Cost, unsigned int> b) {
590 return obj_->isCostBetterThan(std::get<1>(b), std::get<1>(a));
591 });
592 threshold = dists.back() * snapToVertex;
593 result = true;
594 nochange = 0;
595 }
596 }
597 si->freeState(perturb_state);
598 si->freeState(before_state);
599 si->freeState(after_state);
600 si->freeState(new_state);
601
602 return result;
603}
604
606 unsigned int maxEmptySteps)
607{
608 if (path.getStateCount() < 3)
609 return false;
610
611 if (maxSteps == 0)
612 maxSteps = path.getStateCount();
613
614 if (maxEmptySteps == 0)
615 maxEmptySteps = path.getStateCount();
616
617 const base::SpaceInformationPtr &si = path.getSpaceInformation();
618 std::vector<base::State *> &states = path.getStates();
619
620 // compute pair-wise distances in path (construct only half the matrix)
621 std::map<std::pair<const base::State *, const base::State *>, double> distances;
622 for (unsigned int i = 0; i < states.size(); ++i)
623 for (unsigned int j = i + 2; j < states.size(); ++j)
624 distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
625
626 bool result = false;
627 unsigned int nochange = 0;
628 for (unsigned int s = 0; s < maxSteps && nochange < maxEmptySteps; ++s, ++nochange)
629 {
630 // find closest pair of points
631 double minDist = std::numeric_limits<double>::infinity();
632 int p1 = -1;
633 int p2 = -1;
634 for (unsigned int i = 0; i < states.size(); ++i)
635 for (unsigned int j = i + 2; j < states.size(); ++j)
636 {
637 double d = distances[std::make_pair(states[i], states[j])];
638 if (d < minDist)
639 {
640 minDist = d;
641 p1 = i;
642 p2 = j;
643 }
644 }
645
646 if (p1 >= 0 && p2 >= 0)
647 {
648 if (si->checkMotion(states[p1], states[p2]))
649 {
650 if (freeStates_)
651 for (int i = p1 + 1; i < p2; ++i)
652 si->freeState(states[i]);
653 states.erase(states.begin() + p1 + 1, states.begin() + p2);
654 result = true;
655 nochange = 0;
656 }
657 else
658 distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
659 }
660 else
661 break;
662 }
663 return result;
664}
665
667{
669 return simplify(path, neverTerminate);
670}
671
672bool ompl::geometric::PathSimplifier::simplify(PathGeometric &path, double maxTime, bool atLeastOnce)
673{
674 return simplify(path, base::timedPlannerTerminationCondition(maxTime), atLeastOnce);
675}
676
678 bool atLeastOnce)
679{
680 if (path.getStateCount() < 3)
681 return true;
682
683 bool tryMore = true, valid = true;
684 while ((ptc == false || atLeastOnce) && tryMore)
685 {
686 // if the space is metric, we can do some additional smoothing
687 if ((ptc == false || atLeastOnce) && si_->getStateSpace()->isMetricSpace())
688 {
689 bool metricTryMore = true;
690 unsigned int times = 0;
691 do
692 {
693 bool shortcut = shortcutPath(path); // split path segments, not just vertices
694 bool better_goal =
695 gsr_ ? findBetterGoal(path, ptc) : false; // Try to connect the path to a closer goal
696
697 metricTryMore = shortcut || better_goal;
698 } while ((ptc == false || atLeastOnce) && ++times <= 5 && metricTryMore);
699
700 // smooth the path with BSpline interpolation
701 if (ptc == false || atLeastOnce)
702 smoothBSpline(path, 3, path.length() / 100.0);
703
704 if (ptc == false || atLeastOnce)
705 {
706 // we always run this if the metric-space algorithms were run. In non-metric spaces this does not work.
707 const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
708 if (!p.second)
709 {
710 valid = false;
711 OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
712 }
713 else if (!p.first)
714 OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but "
715 "it was "
716 "successfully fixed.");
717 }
718 }
719
720 // try a randomized step of connecting vertices
721 if (ptc == false || atLeastOnce)
722 tryMore = reduceVertices(path);
723
724 // try to collapse close-by vertices
725 if (ptc == false || atLeastOnce)
726 collapseCloseVertices(path);
727
728 // try to reduce verices some more, if there is any point in doing so
729 unsigned int times = 0;
730 while ((ptc == false || atLeastOnce) && tryMore && ++times <= 5)
731 tryMore = reduceVertices(path);
732
733 if ((ptc == false || atLeastOnce) && si_->getStateSpace()->isMetricSpace())
734 {
735 // we always run this if the metric-space algorithms were run. In non-metric spaces this does not work.
736 const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
737 if (!p.second)
738 {
739 valid = false;
740 OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
741 }
742 else if (!p.first)
743 OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it "
744 "was "
745 "successfully fixed.");
746 }
747
748 atLeastOnce = false;
749 }
750 return valid || path.check();
751}
752
753bool ompl::geometric::PathSimplifier::findBetterGoal(PathGeometric &path, double maxTime, unsigned int samplingAttempts,
754 double rangeRatio, double snapToVertex)
755{
756 return findBetterGoal(path, base::timedPlannerTerminationCondition(maxTime), samplingAttempts, rangeRatio,
757 snapToVertex);
758}
759
761 unsigned int samplingAttempts, double rangeRatio,
762 double snapToVertex)
763{
764 if (path.getStateCount() < 2)
765 return false;
766
767 if (!gsr_)
768 {
769 OMPL_WARN("%s: No goal sampleable object to sample a better goal from.", "PathSimplifier::findBetterGoal");
770 return false;
771 }
772
773 unsigned int maxGoals = std::min((unsigned)10, gsr_->maxSampleCount()); // the number of goals we will sample
774 unsigned int failedTries = 0;
775 bool betterGoal = false;
776
777 const base::StateSpacePtr &ss = si_->getStateSpace();
778 std::vector<base::State *> &states = path.getStates();
779
780 // dists[i] contains the cumulative length of the path up to and including state i
781 std::vector<base::Cost> costs(states.size(), obj_->identityCost());
782 std::vector<double> dists(states.size(), 0.0);
783 for (unsigned int i = 1; i < dists.size(); ++i)
784 {
785 costs[i] = obj_->combineCosts(costs[i - 1], obj_->motionCost(states[i - 1], states[i]));
786 dists[i] = dists[i - 1] + si_->distance(states[i - 1], states[i]);
787 if (dists[i] < 0)
788 {
789 OMPL_WARN("%s: Somehow computed negative distance!.", "PathSimplifier::findBetterGoal");
790 return false;
791 }
792 }
793
794 // Sampled states closer than 'threshold' distance to any existing state in the path
795 // are snapped to the close state
796 double threshold = dists.back() * snapToVertex;
797 // The range (distance) of a single connection that will be attempted
798 double rd = rangeRatio * dists.back();
799
800 base::State *temp = si_->allocState();
801 base::State *tempGoal = si_->allocState();
802
803 while (!ptc && failedTries++ < maxGoals && !betterGoal)
804 {
805 gsr_->sampleGoal(tempGoal);
806
807 // Goal state is not compatible with the start state
808 if (!gsr_->isStartGoalPairValid(path.getState(0), tempGoal))
809 continue;
810
811 unsigned int numSamples = 0;
812 while (!ptc && numSamples++ < samplingAttempts && !betterGoal)
813 {
814 // sample a state within rangeRatio
815 double t = rng_.uniformReal(std::max(dists.back() - rd, 0.0),
816 dists.back()); // Sample a random point within rd of the end of the path
817
818 auto end = std::lower_bound(dists.begin(), dists.end(), t);
819 auto start = end;
820 while (start != dists.begin() && *start >= t)
821 start -= 1;
822
823 unsigned int startIndex = start - dists.begin();
824 unsigned int endIndex = end - dists.begin();
825
826 // Snap the random point to the nearest vertex, if within the threshold
827 if (t - (*start) < threshold) // snap to the starting waypoint
828 endIndex = startIndex;
829 if ((*end) - t < threshold) // snap to the ending waypoint
830 startIndex = endIndex;
831
832 // Compute the state value and the accumulated cost up to that state
833 base::Cost costToCome = costs[startIndex];
834 base::State *state;
835 if (startIndex == endIndex)
836 {
837 state = states[startIndex];
838 }
839 else
840 {
841 double tSeg = (t - (*start)) / (*end - *start);
842 ss->interpolate(states[startIndex], states[endIndex], tSeg, temp);
843 state = temp;
844
845 costToCome = obj_->combineCosts(costToCome, obj_->motionCost(states[startIndex], state));
846 }
847
848 base::Cost costToGo = obj_->motionCost(state, tempGoal);
849 base::Cost candidateCost = obj_->combineCosts(costToCome, costToGo);
850
851 // Make sure we improve before attempting validation
852 if (obj_->isCostBetterThan(candidateCost, costs.back()) && si_->checkMotion(state, tempGoal))
853 {
854 // insert the new states
855 if (startIndex == endIndex)
856 {
857 // new intermediate state
858 si_->copyState(states[startIndex], state);
859 // new goal state
860 si_->copyState(states[startIndex + 1], tempGoal);
861
862 if (freeStates_)
863 {
864 for (size_t i = startIndex + 2; i < states.size(); ++i)
865 si_->freeState(states[i]);
866 }
867 states.erase(states.begin() + startIndex + 2, states.end());
868 }
869 else
870 {
871 // overwriting the end of the segment with the new state
872 si_->copyState(states[endIndex], state);
873 if (endIndex == states.size() - 1)
874 {
875 path.append(tempGoal);
876 }
877 else
878 {
879 // adding goal new goal state
880 si_->copyState(states[endIndex + 1], tempGoal);
881 if (freeStates_)
882 {
883 for (size_t i = endIndex + 2; i < states.size(); ++i)
884 si_->freeState(states[i]);
885 }
886 states.erase(states.begin() + endIndex + 2, states.end());
887 }
888 }
889
890 // fix the helper variables
891 costs.resize(states.size(), obj_->identityCost());
892 dists.resize(states.size(), 0.0);
893 for (unsigned int j = std::max(1u, startIndex); j < dists.size(); ++j)
894 {
895 costs[j] = obj_->combineCosts(costs[j - 1], obj_->motionCost(states[j - 1], states[j]));
896 dists[j] = dists[j - 1] + si_->distance(states[j - 1], states[j]);
897 }
898
899 betterGoal = true;
900 }
901 }
902 }
903
904 si_->freeState(temp);
905 si_->freeState(tempGoal);
906
907 return betterGoal;
908}
909
910int ompl::geometric::PathSimplifier::selectAlongPath(std::vector<double> dists, std::vector<base::State *> states,
911 double distTo, double threshold, base::State *select_state,
912 int &pos)
913{
914 if (distTo < 0)
915 distTo = 0;
916 else if (distTo > dists.back())
917 distTo = dists.back();
918
919 int index = -1;
920 auto pit = std::lower_bound(dists.begin(), dists.end(), distTo);
921 pos = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
922
923 if (pos == 0 || dists[pos] - distTo < threshold)
924 index = pos;
925 else
926 {
927 while (pos > 0 && distTo < dists[pos])
928 --pos;
929 if (distTo - dists[pos] < threshold)
930 index = pos;
931 }
932
933 if (index >= 0)
934 {
935 si_->copyState(select_state, states[index]);
936 return index;
937 }
938 else
939 {
940 double t = (distTo - dists[pos]) / (dists[pos + 1] - dists[pos]);
941 si_->getStateSpace()->interpolate(states[pos], states[pos + 1], t, select_state);
942 return -1;
943 }
944}
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition of an abstract state.
Definition: State.h:50
Definition of a geometric path.
Definition: PathGeometric.h:66
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 perturbPath(PathGeometric &path, double stepSize, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double snapToVertex=0.005)
Given a path, attempt to improve the cost by randomly perturbing a randomly selected point on the pat...
base::SpaceInformationPtr si_
The space information this path simplifier uses.
bool simplify(PathGeometric &path, double maxTime, bool atLeastOnce=true)
Run simplification algorithms on the path for at most maxTime seconds, and at least once if atLeastOn...
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...
bool simplifyMax(PathGeometric &path)
Given a path, attempt to remove vertices from it while keeping the path valid. Then,...
bool freeStates() const
Return true if the memory of states is freed when they are removed from a path during simplification.
base::OptimizationObjectivePtr obj_
The optimization objective to use when making improvements. Will be used on all methods except reduce...
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 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).
PathSimplifier(base::SpaceInformationPtr si, const base::GoalPtr &goal=ompl::base::GoalPtr(), const base::OptimizationObjectivePtr &obj=nullptr)
Create an instance for a specified space information. Optionally, a GoalSampleableRegion may be passe...
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...
std::shared_ptr< base::GoalSampleableRegion > gsr_
The goal object for the path simplifier. Used for end-of-path improvements.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
PlannerTerminationCondition plannerNonTerminatingCondition()
Simple termination condition that always returns false. The termination condition will never be met.
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
static const unsigned int MAX_VALID_SAMPLE_ATTEMPTS
When multiple attempts are needed to generate valid samples, this value defines the default number of...
STL namespace.