SpaceInformation.cpp
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 #include "ompl/control/SpaceInformation.h"
38 #include "ompl/control/SimpleDirectedControlSampler.h"
39 #include "ompl/control/SteeredControlSampler.h"
40 #include "ompl/util/Exception.h"
41 #include <cassert>
42 #include <utility>
43 #include <limits>
44 
46 {
48  if (!statePropagator_)
49  throw Exception("State propagator not defined");
50  if (minSteps_ > maxSteps_)
51  throw Exception("The minimum number of steps cannot be larger than the maximum number of steps");
52  if (minSteps_ == 0 && maxSteps_ == 0)
53  {
54  minSteps_ = 1;
55  maxSteps_ = 10;
56  OMPL_WARN("Assuming propagation will always have between %d and %d steps", minSteps_, maxSteps_);
57  }
58  if (minSteps_ < 1)
59  throw Exception("The minimum number of steps must be at least 1");
60 
61  if (stepSize_ < std::numeric_limits<double>::epsilon())
62  {
64  if (stepSize_ < std::numeric_limits<double>::epsilon())
65  throw Exception("The propagation step size must be larger than 0");
66  OMPL_WARN("The propagation step size is assumed to be %f", stepSize_);
67  }
68 
69  controlSpace_->setup();
70  if (controlSpace_->getDimension() <= 0)
71  throw Exception("The dimension of the control space we plan in must be > 0");
72 }
73 
75 {
76  if (dcsa_)
77  return dcsa_(this);
78  else
81 }
82 
84 {
85  dcsa_ = dcsa;
86  setup_ = false;
87 }
88 
90 {
92  setup_ = false;
93 }
94 
96 {
97  class BoostFnStatePropagator : public StatePropagator
98  {
99  public:
100 
101  BoostFnStatePropagator(SpaceInformation *si, const StatePropagatorFn &fn) : StatePropagator(si), fn_(fn)
102  {
103  }
104 
105  virtual void propagate(const base::State *state, const Control *control, const double duration, base::State *result) const
106  {
107  fn_(state, control, duration, result);
108  }
109 
110  protected:
111 
112  StatePropagatorFn fn_;
113 
114  };
115 
116  setStatePropagator(StatePropagatorPtr(dynamic_cast<StatePropagator*>(new BoostFnStatePropagator(this, fn))));
117 }
118 
119 void ompl::control::SpaceInformation::setStatePropagator(const StatePropagatorPtr &sp)
120 {
121  statePropagator_ = sp;
122 }
123 
125 {
126  return statePropagator_->canPropagateBackward();
127 }
128 
129 void ompl::control::SpaceInformation::propagate(const base::State *state, const Control *control, int steps, base::State *result) const
130 {
131  if (steps == 0)
132  {
133  if (result != state)
134  copyState(result, state);
135  }
136  else
137  {
138  double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
139  steps = abs(steps);
140 
141  statePropagator_->propagate(state, control, signedStepSize, result);
142  for (int i = 1 ; i < steps ; ++i)
143  statePropagator_->propagate(result, control, signedStepSize, result);
144  }
145 }
146 
147 unsigned int ompl::control::SpaceInformation::propagateWhileValid(const base::State *state, const Control *control, int steps, base::State *result) const
148 {
149  if (steps == 0)
150  {
151  if (result != state)
152  copyState(result, state);
153  return 0;
154  }
155 
156  double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
157  steps = abs(steps);
158 
159  // perform the first step of propagation
160  statePropagator_->propagate(state, control, signedStepSize, result);
161 
162  // if we found a valid state after one step, we can go on
163  if (isValid(result))
164  {
165  base::State *temp1 = result;
166  base::State *temp2 = allocState();
167  base::State *toDelete = temp2;
168  unsigned int r = steps;
169 
170  // for the remaining number of steps
171  for (int i = 1 ; i < steps ; ++i)
172  {
173  statePropagator_->propagate(temp1, control, signedStepSize, temp2);
174  if (isValid(temp2))
175  std::swap(temp1, temp2);
176  else
177  {
178  // the last valid state is temp1;
179  r = i;
180  break;
181  }
182  }
183 
184  // if we finished the for-loop without finding an invalid state, the last valid state is temp1
185  // make sure result contains that information
186  if (result != temp1)
187  copyState(result, temp1);
188 
189  // free the temporary memory
190  freeState(toDelete);
191 
192  return r;
193  }
194  // if the first propagation step produced an invalid step, return 0 steps
195  // the last valid state is the starting one (assumed to be valid)
196  else
197  {
198  if (result != state)
199  copyState(result, state);
200  return 0;
201  }
202 }
203 
204 void ompl::control::SpaceInformation::propagate(const base::State *state, const Control *control, int steps, std::vector<base::State*> &result, bool alloc) const
205 {
206  double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
207  steps = abs(steps);
208 
209  if (alloc)
210  {
211  result.resize(steps);
212  for (unsigned int i = 0 ; i < result.size() ; ++i)
213  result[i] = allocState();
214  }
215  else
216  {
217  if (result.empty())
218  return;
219  steps = std::min(steps, (int)result.size());
220  }
221 
222  int st = 0;
223 
224  if (st < steps)
225  {
226  statePropagator_->propagate(state, control, signedStepSize, result[st]);
227  ++st;
228 
229  while (st < steps)
230  {
231  statePropagator_->propagate(result[st-1], control, signedStepSize, result[st]);
232  ++st;
233  }
234  }
235 }
236 
237 unsigned int ompl::control::SpaceInformation::propagateWhileValid(const base::State *state, const Control *control, int steps, std::vector<base::State*> &result, bool alloc) const
238 {
239  double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
240  steps = abs(steps);
241 
242  if (alloc)
243  result.resize(steps);
244  else
245  {
246  if (result.empty())
247  return 0;
248  steps = std::min(steps, (int)result.size());
249  }
250 
251  int st = 0;
252 
253  if (st < steps)
254  {
255  if (alloc)
256  result[st] = allocState();
257  statePropagator_->propagate(state, control, signedStepSize, result[st]);
258 
259  if (isValid(result[st]))
260  {
261  ++st;
262  while (st < steps)
263  {
264  if (alloc)
265  result[st] = allocState();
266  statePropagator_->propagate(result[st-1], control, signedStepSize, result[st]);
267 
268  if (!isValid(result[st]))
269  {
270  if (alloc)
271  {
272  freeState(result[st]);
273  result.resize(st);
274  }
275  break;
276  }
277  else
278  ++st;
279  }
280  }
281  else
282  {
283  if (alloc)
284  {
285  freeState(result[st]);
286  result.resize(st);
287  }
288  }
289  }
290 
291  return st;
292 }
293 
295 {
297  out << " - control space:" << std::endl;
298  controlSpace_->printSettings(out);
299  out << " - can propagate backward: " << (canPropagateBackward() ? "yes" : "no") << std::endl;
300  out << " - propagation step size: " << stepSize_ << std::endl;
301  out << " - propagation duration: [" << minSteps_ << ", " << maxSteps_ << "]" << std::endl;
302 }
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 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...
Definition of an abstract control.
Definition: Control.h:48
void freeState(State *state) const
Free the memory of a state.
StatePropagatorPtr statePropagator_
The state propagator used to model the motion of the system being planned for.
bool isValid(const State *state) const
Check if a given state is valid or not.
State * allocState() const
Allocate memory for a state.
double getStateValidityCheckingResolution() const
Get the resolution at which state validity is verified. This call is only applicable if a ompl::base:...
Model the effect of controls on system states.
DirectedControlSamplerAllocator dcsa_
Optional allocator for the DirectedControlSampler. If not specified, the default implementation is us...
boost::function< DirectedControlSamplerPtr(const SpaceInformation *)> DirectedControlSamplerAllocator
Definition of a function that can allocate a directed control sampler.
Abstract definition of a steered control sampler. It uses the steering function in a state propagator...
bool canPropagateBackward() const
Some systems can only propagate forward in time (i.e., the steps argument for the propagate() functio...
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...
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
void copyState(State *destination, const State *source) const
Copy a state to another.
bool setup_
Flag indicating whether setup() has been called on this instance.
Implementation of a simple directed control sampler. This is a basic implementation that does not act...
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.
virtual void setup()
Perform additional setup tasks (run once, before use). If state validity checking resolution has not ...
unsigned int maxSteps_
The maximum number of steps to apply a control for.
Definition of an abstract state.
Definition: State.h:50
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
unsigned int minSteps_
The minimum number of steps to apply a control for.
The exception type for ompl.
Definition: Exception.h:47
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
void setStatePropagator(const StatePropagatorFn &fn)
Set the function that performs state propagation.
double getMaximumExtent() const
Get the maximum extent of the space we are planning in. This is the maximum distance that could be re...
Space information containing necessary information for planning with controls. setup() needs to be ca...
virtual void setup()
Perform additional setup tasks (run once, before use)