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  if (statePropagator_->canSteer())
79  return std::make_shared<SteeredControlSampler>(this);
80  else
81  return std::make_shared<SimpleDirectedControlSampler>(this);
82 }
83 
85 {
86  dcsa_ = dcsa;
87  setup_ = false;
88 }
89 
91 {
93  setup_ = false;
94 }
95 
97 {
98  class FnStatePropagator : public StatePropagator
99  {
100  public:
101  FnStatePropagator(SpaceInformation *si, StatePropagatorFn fn) : StatePropagator(si), fn_(std::move(fn))
102  {
103  }
104 
105  void propagate(const base::State *state, const Control *control, const double duration,
106  base::State *result) const override
107  {
108  fn_(state, control, duration, result);
109  }
110 
111  protected:
112  StatePropagatorFn fn_;
113  };
114 
115  setStatePropagator(std::make_shared<FnStatePropagator>(this, fn));
116 }
117 
118 void ompl::control::SpaceInformation::setStatePropagator(const StatePropagatorPtr &sp)
119 {
120  statePropagator_ = sp;
121 }
122 
124 {
125  return statePropagator_->canPropagateBackward();
126 }
127 
128 void ompl::control::SpaceInformation::propagate(const base::State *state, const Control *control, int steps,
129  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 
148  int steps, base::State *result) const
149 {
150  if (steps == 0)
151  {
152  if (result != state)
153  copyState(result, state);
154  return 0;
155  }
156 
157  double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
158  steps = abs(steps);
159 
160  // perform the first step of propagation
161  statePropagator_->propagate(state, control, signedStepSize, result);
162 
163  // if we found a valid state after one step, we can go on
164  if (isValid(result))
165  {
166  base::State *temp1 = result;
167  base::State *temp2 = allocState();
168  base::State *toDelete = temp2;
169  unsigned int r = steps;
170 
171  // for the remaining number of steps
172  for (int i = 1; i < steps; ++i)
173  {
174  statePropagator_->propagate(temp1, control, signedStepSize, temp2);
175  if (isValid(temp2))
176  std::swap(temp1, temp2);
177  else
178  {
179  // the last valid state is temp1;
180  r = i;
181  break;
182  }
183  }
184 
185  // if we finished the for-loop without finding an invalid state, the last valid state is temp1
186  // make sure result contains that information
187  if (result != temp1)
188  copyState(result, temp1);
189 
190  // free the temporary memory
191  freeState(toDelete);
192 
193  return r;
194  }
195  // if the first propagation step produced an invalid step, return 0 steps
196  // the last valid state is the starting one (assumed to be valid)
197  if (result != state)
198  copyState(result, state);
199  return 0;
200 }
201 
202 void ompl::control::SpaceInformation::propagate(const base::State *state, const Control *control, int steps,
203  std::vector<base::State *> &result, bool alloc) const
204 {
205  double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
206  steps = abs(steps);
207 
208  if (alloc)
209  {
210  result.resize(steps);
211  for (auto &i : result)
212  i = allocState();
213  }
214  else
215  {
216  if (result.empty())
217  return;
218  steps = std::min(steps, (int)result.size());
219  }
220 
221  int st = 0;
222 
223  if (st < steps)
224  {
225  statePropagator_->propagate(state, control, signedStepSize, result[st]);
226  ++st;
227 
228  while (st < steps)
229  {
230  statePropagator_->propagate(result[st - 1], control, signedStepSize, result[st]);
231  ++st;
232  }
233  }
234 }
235 
237  int steps, std::vector<base::State *> &result,
238  bool alloc) const
239 {
240  double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
241  steps = abs(steps);
242 
243  if (alloc)
244  result.resize(steps);
245  else
246  {
247  if (result.empty())
248  return 0;
249  steps = std::min(steps, (int)result.size());
250  }
251 
252  int st = 0;
253 
254  if (st < steps)
255  {
256  if (alloc)
257  result[st] = allocState();
258  statePropagator_->propagate(state, control, signedStepSize, result[st]);
259 
260  if (isValid(result[st]))
261  {
262  ++st;
263  while (st < steps)
264  {
265  if (alloc)
266  result[st] = allocState();
267  statePropagator_->propagate(result[st - 1], control, signedStepSize, result[st]);
268 
269  if (!isValid(result[st]))
270  {
271  if (alloc)
272  {
273  freeState(result[st]);
274  result.resize(st);
275  }
276  break;
277  }
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.
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:47
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.
std::function< DirectedControlSamplerPtr(const SpaceInformation *)> DirectedControlSamplerAllocator
Definition of a function that can allocate a directed control sampler.
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...
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...
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...
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.
void setDirectedControlSamplerAllocator(const DirectedControlSamplerAllocator &dcsa)
Set the allocator to use for the DirectedControlSampler.
A shared pointer wrapper for ompl::control::DirectedControlSampler.
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.
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:49
#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:46
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...