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  const base::StateSpacePtr &stateSpace, ControlSpacePtr controlSpace)
47  : base::SpaceInformation(stateSpace), controlSpace_(std::move(controlSpace))
48 {
49  declareParams();
50 }
51 
53 {
54  params_.declareParam<unsigned int>("min_control_duration",
55  [this](unsigned int n) { setMinControlDuration(n); },
56  [this] { return getMinControlDuration(); });
57  params_.declareParam<unsigned int>("max_control_duration",
58  [this](unsigned int n) { setMaxControlDuration(n); },
59  [this] { return getMaxControlDuration(); });
60  params_.declareParam<double>("propagation_step_size",
61  [this](double s) { setPropagationStepSize(s); },
62  [this] { return getPropagationStepSize(); });
63 }
64 
66 {
68  declareParams(); // calling base::SpaceInformation::setup() clears the params
69  if (!statePropagator_)
70  throw Exception("State propagator not defined");
71  if (minSteps_ > maxSteps_)
72  throw Exception("The minimum number of steps cannot be larger than the maximum number of steps");
73  if (minSteps_ == 0 && maxSteps_ == 0)
74  {
75  minSteps_ = 1;
76  maxSteps_ = 10;
77  OMPL_WARN("Assuming propagation will always have between %d and %d steps", minSteps_, maxSteps_);
78  }
79  if (minSteps_ < 1)
80  throw Exception("The minimum number of steps must be at least 1");
81 
82  if (stepSize_ < std::numeric_limits<double>::epsilon())
83  {
84  stepSize_ = getStateValidityCheckingResolution() * getMaximumExtent();
85  if (stepSize_ < std::numeric_limits<double>::epsilon())
86  throw Exception("The propagation step size must be larger than 0");
87  OMPL_WARN("The propagation step size is assumed to be %f", stepSize_);
88  }
89 
90  controlSpace_->setup();
91  if (controlSpace_->getDimension() <= 0)
92  throw Exception("The dimension of the control space we plan in must be > 0");
93 }
94 
96 {
97  if (dcsa_)
98  return dcsa_(this);
99  if (statePropagator_->canSteer())
100  return std::make_shared<SteeredControlSampler>(this);
101  else
102  return std::make_shared<SimpleDirectedControlSampler>(this);
103 }
104 
106 {
107  dcsa_ = dcsa;
108  setup_ = false;
109 }
110 
112 {
114  setup_ = false;
115 }
116 
118 {
119  class FnStatePropagator : public StatePropagator
120  {
121  public:
122  FnStatePropagator(SpaceInformation *si, StatePropagatorFn fn) : StatePropagator(si), fn_(std::move(fn))
123  {
124  }
125 
126  void propagate(const base::State *state, const Control *control, const double duration,
127  base::State *result) const override
128  {
129  fn_(state, control, duration, result);
130  }
131 
132  protected:
133  StatePropagatorFn fn_;
134  };
135 
136  setStatePropagator(std::make_shared<FnStatePropagator>(this, fn));
137 }
138 
140 {
141  statePropagator_ = sp;
142 }
143 
145 {
146  return statePropagator_->canPropagateBackward();
147 }
148 
149 void ompl::control::SpaceInformation::propagate(const base::State *state, const Control *control, int steps,
150  base::State *result) const
151 {
152  if (steps == 0)
153  {
154  if (result != state)
155  copyState(result, state);
156  }
157  else
158  {
159  double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
160  steps = abs(steps);
161 
162  statePropagator_->propagate(state, control, signedStepSize, result);
163  for (int i = 1; i < steps; ++i)
164  statePropagator_->propagate(result, control, signedStepSize, result);
165  }
166 }
167 
169  int steps, base::State *result) const
170 {
171  if (steps == 0)
172  {
173  if (result != state)
174  copyState(result, state);
175  return 0;
176  }
177 
178  double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
179  steps = abs(steps);
180 
181  // perform the first step of propagation
182  statePropagator_->propagate(state, control, signedStepSize, result);
183 
184  // if we found a valid state after one step, we can go on
185  if (isValid(result))
186  {
187  base::State *temp1 = result;
188  base::State *temp2 = allocState();
189  base::State *toDelete = temp2;
190  unsigned int r = steps;
191 
192  // for the remaining number of steps
193  for (int i = 1; i < steps; ++i)
194  {
195  statePropagator_->propagate(temp1, control, signedStepSize, temp2);
196  if (isValid(temp2))
197  std::swap(temp1, temp2);
198  else
199  {
200  // the last valid state is temp1;
201  r = i;
202  break;
203  }
204  }
205 
206  // if we finished the for-loop without finding an invalid state, the last valid state is temp1
207  // make sure result contains that information
208  if (result != temp1)
209  copyState(result, temp1);
210 
211  // free the temporary memory
212  freeState(toDelete);
213 
214  return r;
215  }
216  // if the first propagation step produced an invalid step, return 0 steps
217  // the last valid state is the starting one (assumed to be valid)
218  if (result != state)
219  copyState(result, state);
220  return 0;
221 }
222 
223 void ompl::control::SpaceInformation::propagate(const base::State *state, const Control *control, int steps,
224  std::vector<base::State *> &result, bool alloc) const
225 {
226  double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
227  steps = abs(steps);
228 
229  if (alloc)
230  {
231  result.resize(steps);
232  for (auto &i : result)
233  i = allocState();
234  }
235  else
236  {
237  if (result.empty())
238  return;
239  steps = std::min(steps, (int)result.size());
240  }
241 
242  int st = 0;
243 
244  if (st < steps)
245  {
246  statePropagator_->propagate(state, control, signedStepSize, result[st]);
247  ++st;
248 
249  while (st < steps)
250  {
251  statePropagator_->propagate(result[st - 1], control, signedStepSize, result[st]);
252  ++st;
253  }
254  }
255 }
256 
258  int steps, std::vector<base::State *> &result,
259  bool alloc) const
260 {
261  double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
262  steps = abs(steps);
263 
264  if (alloc)
265  result.resize(steps);
266  else
267  {
268  if (result.empty())
269  return 0;
270  steps = std::min(steps, (int)result.size());
271  }
272 
273  int st = 0;
274 
275  if (st < steps)
276  {
277  if (alloc)
278  result[st] = allocState();
279  statePropagator_->propagate(state, control, signedStepSize, result[st]);
280 
281  if (isValid(result[st]))
282  {
283  ++st;
284  while (st < steps)
285  {
286  if (alloc)
287  result[st] = allocState();
288  statePropagator_->propagate(result[st - 1], control, signedStepSize, result[st]);
289 
290  if (!isValid(result[st]))
291  {
292  if (alloc)
293  {
294  freeState(result[st]);
295  result.resize(st);
296  }
297  break;
298  }
299  ++st;
300  }
301  }
302  else
303  {
304  if (alloc)
305  {
306  freeState(result[st]);
307  result.resize(st);
308  }
309  }
310  }
311 
312  return st;
313 }
314 
316 {
318  out << " - control space:" << std::endl;
319  controlSpace_->printSettings(out);
320  out << " - can propagate backward: " << (canPropagateBackward() ? "yes" : "no") << std::endl;
321  out << " - propagation step size: " << stepSize_ << std::endl;
322  out << " - propagation duration: [" << minSteps_ << ", " << maxSteps_ << "]" << std::endl;
323 }
Definition of an abstract control.
Definition: Control.h:48
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...
Definition of an abstract state.
Definition: State.h:50
Model the effect of controls on system states.
void setDirectedControlSamplerAllocator(const DirectedControlSamplerAllocator &dcsa)
Set the allocator to use for the DirectedControlSampler.
std::function< DirectedControlSamplerPtr(const SpaceInformation *)> DirectedControlSamplerAllocator
Definition of a function that can allocate a directed control sampler.
std::function< void(const base::State *, const Control *, const double, base::State *)> StatePropagatorFn
A function that achieves state propagation.
void printSettings(std::ostream &out=std::cout) const override
Print information about the current instance of the state space.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
Space information containing necessary information for planning with controls. setup() needs to be ca...
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
A shared pointer wrapper for ompl::control::DirectedControlSampler.
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.
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 ...
The exception type for ompl.
Definition: Exception.h:47
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)