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 
50 namespace ompl
51 {
54  namespace control
55  {
57 
58  OMPL_CLASS_FORWARD(SpaceInformation);
60 
65  typedef 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  : base::SpaceInformation(stateSpace)
76  , controlSpace_(std::move(controlSpace))
77  , minSteps_(0)
78  , maxSteps_(0)
79  , stepSize_(0.0)
80  {
81  }
82 
83  ~SpaceInformation() override = default;
84 
87  {
88  return controlSpace_;
89  }
90 
96  {
97  return controlSpace_->allocControl();
98  }
99 
101  void freeControl(Control *control) const
102  {
103  controlSpace_->freeControl(control);
104  }
105 
107  void copyControl(Control *destination, const Control *source) const
108  {
109  controlSpace_->copyControl(destination, source);
110  }
111 
113  Control *cloneControl(const Control *source) const
114  {
115  Control *copy = controlSpace_->allocControl();
116  controlSpace_->copyControl(copy, source);
117  return copy;
118  }
119 
126  void printControl(const Control *control, std::ostream &out = std::cout) const
127  {
128  controlSpace_->printControl(control, out);
129  }
130 
132  bool equalControls(const Control *control1, const Control *control2) const
133  {
134  return controlSpace_->equalControls(control1, control2);
135  }
136 
138  void nullControl(Control *control) const
139  {
140  controlSpace_->nullControl(control);
141  }
142 
150  {
151  return controlSpace_->allocControlSampler();
152  }
153 
155  void setMinMaxControlDuration(unsigned int minSteps, unsigned int maxSteps)
156  {
157  minSteps_ = minSteps;
158  maxSteps_ = maxSteps;
159  }
160 
162  unsigned int getMinControlDuration() const
163  {
164  return minSteps_;
165  }
166 
168  unsigned int getMaxControlDuration() const
169  {
170  return maxSteps_;
171  }
172 
177 
180 
183 
190  const StatePropagatorPtr &getStatePropagator() const
191  {
192  return statePropagator_;
193  }
194 
196  void setStatePropagator(const StatePropagatorFn &fn);
197 
199  void setStatePropagator(const StatePropagatorPtr &sp);
200 
203  void setPropagationStepSize(double stepSize)
204  {
205  stepSize_ = stepSize;
206  }
207 
210  double getPropagationStepSize() const
211  {
212  return stepSize_;
213  }
226  void propagate(const base::State *state, const Control *control, int steps, base::State *result) const;
227 
232  bool canPropagateBackward() const;
233 
245  unsigned int propagateWhileValid(const base::State *state, const Control *control, int steps,
246  base::State *result) const;
247 
258  void propagate(const base::State *state, const Control *control, int steps,
259  std::vector<base::State *> &result, bool alloc) const;
260 
279  unsigned int propagateWhileValid(const base::State *state, const Control *control, int steps,
280  std::vector<base::State *> &result, bool alloc) const;
281 
285  void printSettings(std::ostream &out = std::cout) const override;
286 
288  void setup() override;
289 
290  protected:
293 
295  StatePropagatorPtr statePropagator_;
296 
298  unsigned int minSteps_;
299 
301  unsigned int maxSteps_;
302 
306 
308  double stepSize_;
309  };
310  }
311 }
312 
313 #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...
SpaceInformation(const base::StateSpacePtr &stateSpace, ControlSpacePtr controlSpace)
Constructor. Sets the instance of the state and control spaces to plan with.
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.
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:47
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::control::ControlSampler.
STL namespace.
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.
std::function< DirectedControlSamplerPtr(const SpaceInformation *)> DirectedControlSamplerAllocator
Definition of a function that can allocate a directed control sampler.
DirectedControlSamplerAllocator dcsa_
Optional allocator for the DirectedControlSampler. If not specified, the default implementation is us...
A shared pointer wrapper for ompl::control::ControlSpace.
std::function< void(const base::State *, const Control *, const double, base::State *)> StatePropagatorFn
A function that achieves state propagation.
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: AppBase.h:21
void setup() override
Perform additional setup tasks (run once, before use)
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 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 printSettings(std::ostream &out=std::cout) const override
Print information about the current instance of the state space.
void clearDirectedSamplerAllocator()
Reset the DirectedControlSampler to be the default one.
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:49
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.
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.
void printControl(const Control *control, std::ostream &out=std::cout) const
Print a control to a stream.