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 "ompl/base/SpaceInformation.h"
41 #include "ompl/control/ControlSpace.h"
42 #include "ompl/control/ControlSampler.h"
43 #include "ompl/control/DirectedControlSampler.h"
44 #include "ompl/control/StatePropagator.h"
45 #include "ompl/control/Control.h"
46 #include "ompl/util/ClassForward.h"
47 
48 namespace ompl
49 {
50 
53  namespace control
54  {
55 
57 
58  OMPL_CLASS_FORWARD(SpaceInformation);
60 
66  typedef boost::function<void(const base::State*, const Control*, const double, base::State*)> StatePropagatorFn;
67 
70  {
71  public:
72 
74  SpaceInformation(const base::StateSpacePtr &stateSpace, const ControlSpacePtr &controlSpace) :
75  base::SpaceInformation(stateSpace), controlSpace_(controlSpace),
76  minSteps_(0), maxSteps_(0), stepSize_(0.0)
77  {
78  }
79 
80  virtual ~SpaceInformation()
81  {
82  }
83 
86  {
87  return controlSpace_;
88  }
89 
95  {
96  return controlSpace_->allocControl();
97  }
98 
100  void freeControl(Control *control) const
101  {
102  controlSpace_->freeControl(control);
103  }
104 
106  void copyControl(Control *destination, const Control *source) const
107  {
108  controlSpace_->copyControl(destination, source);
109  }
110 
112  Control* cloneControl(const Control *source) const
113  {
114  Control *copy = controlSpace_->allocControl();
115  controlSpace_->copyControl(copy, source);
116  return copy;
117  }
118 
125  void printControl(const Control *control, std::ostream &out = std::cout) const
126  {
127  controlSpace_->printControl(control, out);
128  }
129 
131  bool equalControls(const Control *control1, const Control *control2) const
132  {
133  return controlSpace_->equalControls(control1, control2);
134  }
135 
137  void nullControl(Control *control) const
138  {
139  controlSpace_->nullControl(control);
140  }
141 
149  {
150  return controlSpace_->allocControlSampler();
151  }
152 
154  void setMinMaxControlDuration(unsigned int minSteps, unsigned int maxSteps)
155  {
156  minSteps_ = minSteps;
157  maxSteps_ = maxSteps;
158  }
159 
161  unsigned int getMinControlDuration() const
162  {
163  return minSteps_;
164  }
165 
167  unsigned int getMaxControlDuration() const
168  {
169  return maxSteps_;
170  }
171 
175 
178 
181 
188  const StatePropagatorPtr& getStatePropagator() const
189  {
190  return statePropagator_;
191  }
192 
194  void setStatePropagator(const StatePropagatorFn &fn);
195 
197  void setStatePropagator(const StatePropagatorPtr &sp);
198 
201  void setPropagationStepSize(double stepSize)
202  {
203  stepSize_ = stepSize;
204  }
205 
207  double getPropagationStepSize() const
208  {
209  return stepSize_;
210  }
221  void propagate(const base::State *state, const Control *control, int steps, base::State *result) const;
222 
227  bool canPropagateBackward() const;
228 
236  unsigned int propagateWhileValid(const base::State *state, const Control *control, int steps, base::State *result) const;
237 
246  void propagate(const base::State *state, const Control *control, int steps, std::vector<base::State*> &result, bool alloc) const;
247 
260  unsigned int propagateWhileValid(const base::State *state, const Control *control, int steps, std::vector<base::State*> &result, bool alloc) const;
261 
265  virtual void printSettings(std::ostream &out = std::cout) const;
266 
268  virtual void setup();
269 
270  protected:
271 
274 
276  StatePropagatorPtr statePropagator_;
277 
279  unsigned int minSteps_;
280 
282  unsigned int maxSteps_;
283 
286 
288  double stepSize_;
289 
290  };
291 
292  }
293 
294 }
295 
296 #endif
DirectedControlSamplerPtr allocDirectedControlSampler() const
Allocate an instance of the DirectedControlSampler to use. This will be the default (SimpleDirectedCo...
ControlSpacePtr controlSpace_
The control space describing the space of controls applicable to states in the state space...
double getPropagationStepSize() const
Propagation is performed at integer multiples of a specified step size. This function returns the val...
unsigned int getMinControlDuration() const
Get the minimum number of steps a control is propagated for.
double stepSize_
The actual duration of each step.
boost::function< void(const base::State *, const Control *, const double, base::State *)> StatePropagatorFn
A function that achieves state propagation.
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 nullControl(Control *control) const
Make the control have no effect if it were to be applied to a state for any amount of time...
Definition of an abstract control.
Definition: Control.h:48
A boost shared pointer wrapper for ompl::base::StateSpace.
A boost shared pointer wrapper for ompl::control::ControlSampler.
StatePropagatorPtr statePropagator_
The state propagator used to model the motion of the system being planned for.
ControlSamplerPtr allocControlSampler() const
Allocate a control sampler.
bool equalControls(const Control *control1, const Control *control2) const
Check if two controls are the same.
Control * allocControl() const
Allocate memory for a control.
DirectedControlSamplerAllocator dcsa_
Optional allocator for the DirectedControlSampler. If not specified, the default implementation is us...
A boost shared pointer wrapper for ompl::control::ControlSpace.
boost::function< DirectedControlSamplerPtr(const SpaceInformation *)> DirectedControlSamplerAllocator
Definition of a function that can allocate a directed control sampler.
bool canPropagateBackward() const
Some systems can only propagate forward in time (i.e., the steps argument for the propagate() functio...
Main namespace. Contains everything in this library.
Definition: Cost.h:42
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 copyControl(Control *destination, const Control *source) const
Copy a control to another.
void setDirectedControlSamplerAllocator(const DirectedControlSamplerAllocator &dcsa)
Set the allocator to use for the DirectedControlSampler.
A boost shared pointer wrapper for ompl::control::DirectedControlSampler.
unsigned int getMaxControlDuration() const
Get the maximum number of steps a control is propagated for.
Control * cloneControl(const Control *source) const
Clone a control.
void clearDirectedSamplerAllocator()
Reset the DirectedControlSampler to be the default one.
SpaceInformation(const base::StateSpacePtr &stateSpace, const ControlSpacePtr &controlSpace)
Constructor. Sets the instance of the state and control spaces to plan with.
The base class for space information. This contains all the information about the space planning is d...
unsigned int maxSteps_
The maximum number of steps to apply a control for.
Definition of an abstract state.
Definition: State.h:50
unsigned int minSteps_
The minimum number of steps to apply a control for.
void setPropagationStepSize(double stepSize)
When controls are applied to states, they are applied for a time duration that is an integer multiple...
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
void setMinMaxControlDuration(unsigned int minSteps, unsigned int maxSteps)
Set the minimum and maximum number of steps a control is propagated for.
void setStatePropagator(const StatePropagatorFn &fn)
Set the function that performs state propagation.
void freeControl(Control *control) const
Free the memory of a control.
const ControlSpacePtr & getControlSpace() const
Get the control space.
Space information containing necessary information for planning with controls. setup() needs to be ca...
const StatePropagatorPtr & getStatePropagator() const
Get the instance of StatePropagator that performs state propagation.
virtual void setup()
Perform additional setup tasks (run once, before use)
void printControl(const Control *control, std::ostream &out=std::cout) const
Print a control to a stream.