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