Loading...
Searching...
No Matches
PathSimplifier.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2008, Willow Garage, 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 Willow Garage 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#ifndef OMPL_GEOMETRIC_PATH_SIMPLIFIER_
38#define OMPL_GEOMETRIC_PATH_SIMPLIFIER_
39
40#include "ompl/base/SpaceInformation.h"
41#include "ompl/geometric/PathGeometric.h"
42#include "ompl/base/PlannerTerminationCondition.h"
43#include "ompl/base/OptimizationObjective.h"
44#include "ompl/base/goals/GoalSampleableRegion.h"
45#include "ompl/util/ClassForward.h"
46#include "ompl/util/RandomNumbers.h"
47#include "ompl/util/Console.h"
48#include <limits>
49
50namespace ompl
51{
52 namespace geometric
53 {
55
56 OMPL_CLASS_FORWARD(PathSimplifier);
58
66 {
67 public:
70 PathSimplifier(base::SpaceInformationPtr si, const base::GoalPtr &goal = ompl::base::GoalPtr(), const base::OptimizationObjectivePtr& obj=nullptr);
71
72 virtual ~PathSimplifier() = default;
73
93 bool reduceVertices(PathGeometric &path, unsigned int maxSteps = 0, unsigned int maxEmptySteps = 0,
94 double rangeRatio = 0.33);
95
125 bool shortcutPath(PathGeometric &path, unsigned int maxSteps = 0, unsigned int maxEmptySteps = 0,
126 double rangeRatio = 0.33, double snapToVertex = 0.005);
127
160 bool perturbPath(PathGeometric &path, double stepSize, unsigned int maxSteps = 0, unsigned int maxEmptySteps = 0, double snapToVertex = 0.005);
161
177 bool collapseCloseVertices(PathGeometric &path, unsigned int maxSteps = 0, unsigned int maxEmptySteps = 0);
178
188 void smoothBSpline(PathGeometric &path, unsigned int maxSteps = 5,
189 double minChange = std::numeric_limits<double>::epsilon());
190
196 bool simplifyMax(PathGeometric &path);
197
200 bool simplify(PathGeometric &path, double maxTime, bool atLeastOnce = true);
201
204 bool simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc, bool atLeastOnce = true);
205
219 bool findBetterGoal(PathGeometric &path, double maxTime, unsigned int samplingAttempts = 10,
220 double rangeRatio = 0.33, double snapToVertex = 0.005);
221
236 unsigned int samplingAttempts = 10, double rangeRatio = 0.33,
237 double snapToVertex = 0.005);
238
242 void freeStates(bool flag);
243
246 bool freeStates() const;
247
248 protected:
249
250 int selectAlongPath(std::vector<double> dists, std::vector<base::State *> states,
251 double distTo, double threshold, base::State *select_state, int &pos);
252
254 base::SpaceInformationPtr si_;
255
257 std::shared_ptr<base::GoalSampleableRegion> gsr_;
258
261 base::OptimizationObjectivePtr obj_;
262
265
268 };
269 }
270}
271
272#endif
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
A shared pointer wrapper for ompl::base::Goal.
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.
This class contains routines that attempt to simplify geometric paths.
bool freeStates_
Flag indicating whether the states removed from a motion should be freed.
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...
RNG rng_
Instance of random number generator.
std::shared_ptr< base::GoalSampleableRegion > gsr_
The goal object for the path simplifier. Used for end-of-path improvements.
Main namespace. Contains everything in this library.