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 }