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  using StatePropagatorFn =
66  std::function<void(const base::State *, const Control *, const double, base::State *)>;
67 
70  class SpaceInformation : public base::SpaceInformation
71  {
72  public:
74  SpaceInformation(const base::StateSpacePtr &stateSpace, ControlSpacePtr controlSpace);
75 
76  ~SpaceInformation() override = default;
77 
79  const ControlSpacePtr &getControlSpace() const
80  {
81  return controlSpace_;
82  }
83 
88  Control *allocControl() const
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 
201  void setStatePropagator(const StatePropagatorFn &fn);
202 
204  void setStatePropagator(const StatePropagatorPtr &sp);
205 
208  void setPropagationStepSize(double stepSize)
209  {
210  stepSize_ = stepSize;
211  }
212 
215  double getPropagationStepSize() const
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
void copyControl(Control *destination, const Control *source) const
Copy a control to another.
Definition of an abstract control.
Definition: Control.h:111
Control * cloneControl(const Control *source) const
Clone a control.
void setStatePropagator(const StatePropagatorFn &fn)
Set the function that performs state propagation.
SpaceInformation(const base::StateSpacePtr &stateSpace, ControlSpacePtr controlSpace)
Constructor. Sets the instance of the state and control spaces to plan with.
A shared pointer wrapper for ompl::control::ControlSpace.
DirectedControlSamplerPtr allocDirectedControlSampler() const
Allocate an instance of the DirectedControlSampler to use. This will be the default (SimpleDirectedCo...
Control * allocControl() const
Allocate memory for a control.
Definition of an abstract state.
Definition: State.h:113
void setDirectedControlSamplerAllocator(const DirectedControlSamplerAllocator &dcsa)
Set the allocator to use for the DirectedControlSampler.
unsigned int getMinControlDuration() const
Get 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.
ControlSpacePtr controlSpace_
The control space describing the space of controls applicable to states in the state space.
const StatePropagatorPtr & getStatePropagator() const
Get the instance of StatePropagator that performs state propagation.
std::function< void(const base::State *, const Control *, const double, base::State *)> StatePropagatorFn
A function that achieves state propagation.
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.
DirectedControlSamplerAllocator dcsa_
Optional allocator for the DirectedControlSampler. If not specified, the default implementation is us...
void setMinControlDuration(unsigned int minSteps)
Set the minimum number of steps a control is propagated for.
void setMaxControlDuration(unsigned int maxSteps)
Set the minimum and 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.
void printSettings(std::ostream &out=std::cout) const override
Print information about the current instance of the state space.
unsigned int getMaxControlDuration() const
Get the maximum number of steps a control is propagated for.
double stepSize_
The actual duration of each step.
void freeControl(Control *control) const
Free the memory of a control.
A shared pointer wrapper for ompl::control::ControlSampler.
bool equalControls(const Control *control1, const Control *control2) const
Check if two controls are the same.
unsigned int minSteps_
The minimum number of steps to apply a control for.
A shared pointer wrapper for ompl::control::DirectedControlSampler.
void printControl(const Control *control, std::ostream &out=std::cout) const
Print a control to a stream.
double getPropagationStepSize() const
Propagation is performed at integer multiples of a specified step size. This function returns the val...
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,...
A shared pointer wrapper for ompl::control::StatePropagator.
const ControlSpacePtr & getControlSpace() const
Get the control space.
StatePropagatorPtr statePropagator_
The state propagator used to model the motion of the system being planned for.
void setPropagationStepSize(double stepSize)
When controls are applied to states, they are applied for a time duration that is an integer multiple...
void clearDirectedSamplerAllocator()
Reset the DirectedControlSampler to be the default one.
ControlSamplerPtr allocControlSampler() const
Allocate a control sampler.
unsigned int maxSteps_
The maximum number of steps to apply a control for.
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,...
bool canPropagateBackward() const
Some systems can only propagate forward in time (i.e., the steps argument for the propagate() functio...
void setup() override
Perform additional setup tasks (run once, before use)
Main namespace. Contains everything in this library.
Definition: AppBase.h:21