Loading...
Searching...
No Matches
PathControl.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2010, Rice University
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 */
36
37#ifndef OMPL_CONTROL_PATH_CONTROL_
38#define OMPL_CONTROL_PATH_CONTROL_
39
40#include "ompl/control/SpaceInformation.h"
41#include "ompl/base/Path.h"
42#include "ompl/geometric/PathGeometric.h"
43#include <vector>
44
45namespace ompl
46{
47 namespace base
48 {
50 OMPL_CLASS_FORWARD(OptimizationObjective);
52 }
53
54 namespace control
55 {
60 class PathControl : public base::Path
61 {
62 public:
64 PathControl(const base::SpaceInformationPtr &si);
65
67 PathControl(const PathControl &path);
68
69 ~PathControl() override
70 {
71 freeMemory();
72 }
73
75 PathControl &operator=(const PathControl &other);
76
78 base::Cost cost(const base::OptimizationObjectivePtr &opt) const override;
79
81 double length() const override;
82
84 bool check() const override;
85
87 void print(std::ostream &out) const override;
97 virtual void printAsMatrix(std::ostream &out) const;
98
102
110 void append(const base::State *state);
111
116 void append(const base::State *state, const Control *control, double duration);
117
120 void interpolate();
121
123 void random();
124
127 bool randomValid(unsigned int attempts);
128
136 std::vector<base::State *> &getStates()
137 {
138 return states_;
139 }
140
143 std::vector<Control *> &getControls()
144 {
145 return controls_;
146 }
147
150 std::vector<double> &getControlDurations()
151 {
152 return controlDurations_;
153 }
154
156 base::State *getState(unsigned int index)
157 {
158 return states_[index];
159 }
160
162 const base::State *getState(unsigned int index) const
163 {
164 return states_[index];
165 }
166
169 Control *getControl(unsigned int index)
170 {
171 return controls_[index];
172 }
173
176 const Control *getControl(unsigned int index) const
177 {
178 return controls_[index];
179 }
180
182 double getControlDuration(unsigned int index) const
183 {
184 return controlDurations_[index];
185 }
186
188 std::size_t getStateCount() const
189 {
190 return states_.size();
191 }
192
195 std::size_t getControlCount() const
196 {
197 return controls_.size();
198 }
199
202 protected:
204 std::vector<base::State *> states_;
205
208 std::vector<Control *> controls_;
209
212 std::vector<double> controlDurations_;
213
215 void freeMemory();
216
218 void copyFrom(const PathControl &other);
219 };
220 }
221}
222
223#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 a path.
Definition Path.h:68
Definition of an abstract state.
Definition State.h:50
Definition of an abstract control.
Definition Control.h:48
Definition of a control path.
Definition PathControl.h:61
const base::State * getState(unsigned int index) const
Get the state located at index along the path.
double getControlDuration(unsigned int index) const
Get the duration of the control at index, which gets applied to the state at index.
void print(std::ostream &out) const override
Print the path to a stream.
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...
bool check() const override
Check if the path is valid.
base::Cost cost(const base::OptimizationObjectivePtr &opt) const override
Not yet implemented.
base::State * getState(unsigned int index)
Get the state located at index along the path.
std::vector< double > & getControlDurations()
Get the control durations used along the path (as a reference, so it can be modified,...
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
bool randomValid(unsigned int attempts)
Set this path to a random valid segment. Sample attempts times for valid segments....
void append(const base::State *state)
Append state to the end of the path; it is assumed state is the first state, so no control is applied...
void random()
Set this path to a random segment.
PathControl & operator=(const PathControl &other)
Assignment operator.
void copyFrom(const PathControl &other)
Copy the content of a path to this one.
double length() const override
The path length (sum of control durations)
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,...
PathControl(const base::SpaceInformationPtr &si)
Constructor.
std::vector< double > controlDurations_
The duration of the control applied at each state. This array contains one element less than the list...
void freeMemory()
Free the memory allocated by the path.
const Control * getControl(unsigned int index) const
Get the control located at index along the path. This is the control that gets applied to the state l...
Control * getControl(unsigned int index)
Get the control located at index along the path. This is the control that gets applied to the state l...
std::size_t getControlCount() const
Get the number of controls applied along this path. This should be equal to getStateCount() - 1 unles...
std::vector< Control * > & getControls()
Get the controls that make up the path (as a reference, so it can be modified, hence the function is ...
std::vector< base::State * > states_
The list of states that make up the path.
std::vector< Control * > controls_
The control applied at each state. This array contains one element less than the list of states.
geometric::PathGeometric asGeometric() const
Convert this path into a geometric path (interpolation is performed and then states are copied)
void interpolate()
Make the path such that all controls are applied for a single time step (computes intermediate states...
Definition of a geometric path.
Main namespace. Contains everything in this library.