Loading...
Searching...
No Matches
SpaceInformation.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_SPACE_INFORMATION_
38#define OMPL_CONTROL_SPACE_INFORMATION_
39
40#include <utility>
41
42#include "ompl/base/SpaceInformation.h"
43#include "ompl/control/ControlSpace.h"
44#include "ompl/control/ControlSampler.h"
45#include "ompl/control/DirectedControlSampler.h"
46#include "ompl/control/StatePropagator.h"
47#include "ompl/control/Control.h"
48#include "ompl/util/ClassForward.h"
49
50namespace ompl
51{
54 namespace control
55 {
57
58 OMPL_CLASS_FORWARD(SpaceInformation);
60
66 std::function<void(const base::State *, const Control *, const double, base::State *)>;
67
71 {
72 public:
74 SpaceInformation(const base::StateSpacePtr &stateSpace, ControlSpacePtr controlSpace);
75
76 ~SpaceInformation() override = default;
77
80 {
81 return controlSpace_;
82 }
83
89 {
90 return controlSpace_->allocControl();
91 }
92
94 void freeControl(Control *control) const
95 {
96 controlSpace_->freeControl(control);
97 }
98
100 void copyControl(Control *destination, const Control *source) const
101 {
102 controlSpace_->copyControl(destination, source);
103 }
104
106 Control *cloneControl(const Control *source) const
107 {
108 Control *copy = controlSpace_->allocControl();
109 controlSpace_->copyControl(copy, source);
110 return copy;
111 }
112
119 void printControl(const Control *control, std::ostream &out = std::cout) const
120 {
121 controlSpace_->printControl(control, out);
122 }
123
125 bool equalControls(const Control *control1, const Control *control2) const
126 {
127 return controlSpace_->equalControls(control1, control2);
128 }
129
131 void nullControl(Control *control) const
132 {
133 controlSpace_->nullControl(control);
134 }
135
143 {
144 return controlSpace_->allocControlSampler();
145 }
146
148 void setMinMaxControlDuration(unsigned int minSteps, unsigned int maxSteps)
149 {
150 minSteps_ = minSteps;
151 maxSteps_ = maxSteps;
152 }
153
155 void setMinControlDuration(unsigned int minSteps)
156 {
157 minSteps_ = minSteps;
158 }
159
161 void setMaxControlDuration(unsigned int maxSteps)
162 {
163 maxSteps_ = maxSteps;
164 }
165
167 unsigned int getMinControlDuration() const
168 {
169 return minSteps_;
170 }
171
173 unsigned int getMaxControlDuration() const
174 {
175 return maxSteps_;
176 }
177
182
185
188
196 {
197 return statePropagator_;
198 }
199
202
205
208 void setPropagationStepSize(double stepSize)
209 {
210 stepSize_ = stepSize;
211 }
212
216 {
217 return stepSize_;
218 }
231 void propagate(const base::State *state, const Control *control, int steps, base::State *result) const;
232
237 bool canPropagateBackward() const;
238
250 unsigned int propagateWhileValid(const base::State *state, const Control *control, int steps,
251 base::State *result) const;
252
263 void propagate(const base::State *state, const Control *control, int steps,
264 std::vector<base::State *> &result, bool alloc) const;
265
284 unsigned int propagateWhileValid(const base::State *state, const Control *control, int steps,
285 std::vector<base::State *> &result, bool alloc) const;
286
290 void printSettings(std::ostream &out = std::cout) const override;
291
293 void setup() override;
294
295 protected:
297 void declareParams();
298
301
304
306 unsigned int minSteps_{0};
307
309 unsigned int maxSteps_{0};
310
314
316 double stepSize_{0.};
317 };
318 }
319}
320
321#endif
The base class for space information. This contains all the information about the space planning is d...
A shared pointer wrapper for ompl::control::StatePropagator.
Definition of an abstract state.
Definition: State.h:50
A shared pointer wrapper for ompl::control::ControlSampler.
A shared pointer wrapper for ompl::control::ControlSpace.
Definition of an abstract control.
Definition: Control.h:48
A shared pointer wrapper for ompl::control::DirectedControlSampler.
Space information containing necessary information for planning with controls. setup() needs to be ca...
DirectedControlSamplerPtr allocDirectedControlSampler() const
Allocate an instance of the DirectedControlSampler to use. This will be the default (SimpleDirectedCo...
void printSettings(std::ostream &out=std::cout) const override
Print information about the current instance of the state space.
Control * cloneControl(const Control *source) const
Clone a control.
void nullControl(Control *control) const
Make the control have no effect if it were to be applied to a state for any amount of time.
unsigned int minSteps_
The minimum number of steps to apply a control for.
const ControlSpacePtr & getControlSpace() const
Get the control space.
DirectedControlSamplerAllocator dcsa_
Optional allocator for the DirectedControlSampler. If not specified, the default implementation is us...
unsigned int getMinControlDuration() const
Get the minimum number of steps a control is propagated for.
ControlSamplerPtr allocControlSampler() const
Allocate a control sampler.
void setStatePropagator(const StatePropagatorFn &fn)
Set the function that performs state propagation.
double getPropagationStepSize() const
Propagation is performed at integer multiples of a specified step size. This function returns the val...
void setup() override
Perform additional setup tasks (run once, before use)
void setPropagationStepSize(double stepSize)
When controls are applied to states, they are applied for a time duration that is an integer multiple...
ControlSpacePtr controlSpace_
The control space describing the space of controls applicable to states in the state space.
Control * allocControl() const
Allocate memory for a control.
void freeControl(Control *control) const
Free the memory of a control.
const StatePropagatorPtr & getStatePropagator() const
Get the instance of StatePropagator that performs state propagation.
bool equalControls(const Control *control1, const Control *control2) const
Check if two controls are the same.
void printControl(const Control *control, std::ostream &out=std::cout) const
Print a control to a stream.
void setDirectedControlSamplerAllocator(const DirectedControlSamplerAllocator &dcsa)
Set the allocator to use for the DirectedControlSampler.
bool canPropagateBackward() const
Some systems can only propagate forward in time (i.e., the steps argument for the propagate() functio...
unsigned int maxSteps_
The maximum number of steps to apply a control for.
void propagate(const base::State *state, const Control *control, int steps, base::State *result) const
Propagate the model of the system forward, starting a a given state, with a given control,...
void setMaxControlDuration(unsigned int maxSteps)
Set the minimum and maximum number of steps a control is propagated for.
void clearDirectedSamplerAllocator()
Reset the DirectedControlSampler to be the default one.
unsigned int getMaxControlDuration() const
Get the maximum number of steps a control is propagated for.
void setMinMaxControlDuration(unsigned int minSteps, unsigned int maxSteps)
Set the minimum and maximum number of steps a control is propagated for.
StatePropagatorPtr statePropagator_
The state propagator used to model the motion of the system being planned for.
double stepSize_
The actual duration of each step.
void copyControl(Control *destination, const Control *source) const
Copy a control to another.
unsigned int propagateWhileValid(const base::State *state, const Control *control, int steps, base::State *result) const
Propagate the model of the system forward, starting at a given state, with a given control,...
void setMinControlDuration(unsigned int minSteps)
Set the minimum number of steps a control is propagated for.
std::function< DirectedControlSamplerPtr(const SpaceInformation *)> DirectedControlSamplerAllocator
Definition of a function that can allocate a directed control sampler.
std::function< void(const base::State *, const Control *, const double, base::State *)> StatePropagatorFn
A function that achieves state propagation.
Main namespace. Contains everything in this library.