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 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  else
198  {
199  if (result != state)
200  copyState(result, state);
201  return 0;
202  }
203 }
204 
205 void ompl::control::SpaceInformation::propagate(const base::State *state, const Control *control, int steps,
206  std::vector<base::State *> &result, bool alloc) const
207 {
208  double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
209  steps = abs(steps);
210 
211  if (alloc)
212  {
213  result.resize(steps);
214  for (auto &i : result)
215  i = allocState();
216  }
217  else
218  {
219  if (result.empty())
220  return;
221  steps = std::min(steps, (int)result.size());
222  }
223 
224  int st = 0;
225 
226  if (st < steps)
227  {
228  statePropagator_->propagate(state, control, signedStepSize, result[st]);
229  ++st;
230 
231  while (st < steps)
232  {
233  statePropagator_->propagate(result[st - 1], control, signedStepSize, result[st]);
234  ++st;
235  }
236  }
237 }
238 
240  int steps, std::vector<base::State *> &result,
241  bool alloc) const
242 {
243  double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
244  steps = abs(steps);
245 
246  if (alloc)
247  result.resize(steps);
248  else
249  {
250  if (result.empty())
251  return 0;
252  steps = std::min(steps, (int)result.size());
253  }
254 
255  int st = 0;
256 
257  if (st < steps)
258  {
259  if (alloc)
260  result[st] = allocState();
261  statePropagator_->propagate(state, control, signedStepSize, result[st]);
262 
263  if (isValid(result[st]))
264  {
265  ++st;
266  while (st < steps)
267  {
268  if (alloc)
269  result[st] = allocState();
270  statePropagator_->propagate(result[st - 1], control, signedStepSize, result[st]);
271 
272  if (!isValid(result[st]))
273  {
274  if (alloc)
275  {
276  freeState(result[st]);
277  result.resize(st);
278  }
279  break;
280  }
281  else
282  ++st;
283  }
284  }
285  else
286  {
287  if (alloc)
288  {
289  freeState(result[st]);
290  result.resize(st);
291  }
292  }
293  }
294 
295  return st;
296 }
297 
299 {
301  out << " - control space:" << std::endl;
302  controlSpace_->printSettings(out);
303  out << " - can propagate backward: " << (canPropagateBackward() ? "yes" : "no") << std::endl;
304  out << " - propagation step size: " << stepSize_ << std::endl;
305  out << " - propagation duration: [" << minSteps_ << ", " << maxSteps_ << "]" << std::endl;
306 }
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...