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
ControlSpacePtr controlSpace_
The control space describing the space of controls applicable to states in the state space...
unsigned int getMinControlDuration() const
Get the minimum number of steps a control is propagated for.
double stepSize_
The actual duration of each step.
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
boost::function< void(const base::State *, const Control *, const double, base::State *)> StatePropagatorFn
A function that achieves state propagation.
Control * allocControl() const
Allocate memory for a control.
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...
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.
bool equalControls(const Control *control1, const Control *control2) const
Check if two controls are the same.
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...
StatePropagatorPtr statePropagator_
The state propagator used to model the motion of the system being planned for.
void freeControl(Control *control) const
Free the memory of 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.
DirectedControlSamplerPtr allocDirectedControlSampler() const
Allocate an instance of the DirectedControlSampler to use. This will be the default (SimpleDirectedCo...
boost::function< DirectedControlSamplerPtr(const SpaceInformation *)> DirectedControlSamplerAllocator
Definition of a function that can allocate a directed control sampler.
double getPropagationStepSize() const
Propagation is performed at integer multiples of a specified step size. This function returns the val...
Main namespace. Contains everything in this library.
Definition: Cost.h:42
void setDirectedControlSamplerAllocator(const DirectedControlSamplerAllocator &dcsa)
Set the allocator to use for the DirectedControlSampler.
A boost shared pointer wrapper for ompl::control::DirectedControlSampler.
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.
ControlSamplerPtr allocControlSampler() const
Allocate a control sampler.
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.
bool canPropagateBackward() const
Some systems can only propagate forward in time (i.e., the steps argument for the propagate() functio...
const ControlSpacePtr & getControlSpace() const
Get the control space.
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...
void setMinMaxControlDuration(unsigned int minSteps, unsigned int maxSteps)
Set the minimum and maximum number of steps a control is propagated for.
unsigned int getMaxControlDuration() const
Get the maximum number of steps a control is propagated for.
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...
void setStatePropagator(const StatePropagatorFn &fn)
Set the function that performs state propagation.
Control * cloneControl(const Control *source) const
Clone a control.
const StatePropagatorPtr & getStatePropagator() const
Get the instance of StatePropagator that performs state propagation.
Space information containing necessary information for planning with controls. setup() needs to be ca...
void printControl(const Control *control, std::ostream &out=std::cout) const
Print a control to a stream.
virtual void setup()
Perform additional setup tasks (run once, before use)
void copyControl(Control *destination, const Control *source) const
Copy a control to another.