Loading...
Searching...
No Matches
PathGeometric.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 */
36
37#ifndef OMPL_GEOMETRIC_PATH_GEOMETRIC_
38#define OMPL_GEOMETRIC_PATH_GEOMETRIC_
39
40#include "ompl/base/SpaceInformation.h"
41#include "ompl/base/Path.h"
42#include <vector>
43#include <utility>
44
45namespace ompl
46{
47 namespace base
48 {
50 OMPL_CLASS_FORWARD(OptimizationObjective);
52 }
53
55 namespace geometric
56 {
58
59 OMPL_CLASS_FORWARD(PathGeometric);
61
66 {
67 public:
69 PathGeometric(const base::SpaceInformationPtr &si) : base::Path(si)
70 {
71 }
72
74 PathGeometric(const PathGeometric &path);
75
77 PathGeometric(const base::SpaceInformationPtr &si, const base::State *state);
78
80 PathGeometric(const base::SpaceInformationPtr &si,
81 const base::State *state1, const base::State *state2);
82
84 PathGeometric(const base::SpaceInformationPtr &si,
85 std::vector<const base::State *> &states);
86
87 ~PathGeometric() override
88 {
89 freeMemory();
90 }
91
94
100 base::Cost cost(const base::OptimizationObjectivePtr &obj) const override;
101
103 double length() const override;
104
106 bool check() const override;
107
126 double smoothness() const;
127
140 double clearance() const;
141
143 void print(std::ostream &out) const override;
144
149 virtual void printAsMatrix(std::ostream &out) const;
150
153
159 void interpolate(unsigned int count);
160
165 void interpolate();
166
168 void subdivide();
169
171 void reverse();
172
184 std::pair<bool, bool> checkAndRepair(unsigned int attempts);
185
196 void overlay(const PathGeometric &over, unsigned int startIndex = 0);
197
199 void append(const base::State *state);
200
212 void append(const PathGeometric &path);
213
215 void prepend(const base::State *state);
216
219 void keepAfter(const base::State *state);
220
223 void keepBefore(const base::State *state);
224
226 void random();
227
230 bool randomValid(unsigned int attempts);
232
235
238 int getClosestIndex(const base::State *state) const;
239
242 std::vector<base::State *> &getStates()
243 {
244 return states_;
245 }
246
248 base::State *getState(unsigned int index)
249 {
250 return states_[index];
251 }
252
254 const base::State *getState(unsigned int index) const
255 {
256 return states_[index];
257 }
258
260 std::size_t getStateCount() const
261 {
262 return states_.size();
263 }
264
266 void clear();
267
269
270 protected:
272 void freeMemory();
273
275 void copyFrom(const PathGeometric &other);
276
278 std::vector<base::State *> states_;
279 };
280 }
281}
282
283#endif
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
Abstract definition of optimization objectives.
Abstract definition of a path.
Definition Path.h:68
Definition of an abstract state.
Definition State.h:50
Definition of a geometric path.
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
double smoothness() const
Compute a notion of smoothness for this path. The closer the value is to 0, the smoother the path....
bool randomValid(unsigned int attempts)
Set this path to a random valid segment. Sample attempts times for valid segments....
bool check() const override
Check if the path is valid.
const base::State * getState(unsigned int index) const
Get the state located at index along the path.
double clearance() const
Compute the clearance of the way-points along the path (no interpolation is performed)....
void print(std::ostream &out) const override
Print the path to a stream.
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 keepAfter(const base::State *state)
Keep the part of the path that is after state (getClosestIndex() is used to find out which way-point ...
virtual void printAsMatrix(std::ostream &out) const
Print the path as a real-valued matrix where the i-th row represents the i-th state along the path....
void freeMemory()
Free the memory corresponding to the states on this path.
PathGeometric & operator=(const PathGeometric &other)
Assignment operator.
void prepend(const base::State *state)
Prepend state to the start of this path. The memory for state is copied.
void clear()
Remove all states and clear memory.
void subdivide()
Add a state at the middle of each segment.
void random()
Set this path to a random segment.
void reverse()
Reverse the path.
void keepBefore(const base::State *state)
Keep the part of the path that is before state (getClosestIndex() is used to find out which way-point...
void copyFrom(const PathGeometric &other)
Copy data to this path from another path instance.
void interpolate()
Insert a number of states in a path so that the path is made up of (approximately) the states checked...
double length() const override
Compute the length of a geometric path (sum of lengths of segments that make up the path)
base::State * getState(unsigned int index)
Get the state located at index along the path.
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
base::Cost cost(const base::OptimizationObjectivePtr &obj) const override
The sum of the costs for the sequence of segments that make up the path, computed using OptimizationO...
PathGeometric(const base::SpaceInformationPtr &si)
Construct a path instance for a given space information.
std::vector< base::State * > states_
The list of states that make up the path.
int getClosestIndex(const base::State *state) const
Get the index of the way-point along the path that is closest to state. Returns -1 for an empty path.
std::vector< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
void overlay(const PathGeometric &over, unsigned int startIndex=0)
Overlay the path over on top of the current path. States are added to the current path if needed (by ...
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
Main namespace. Contains everything in this library.