Loading...
Searching...
No Matches
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{
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:
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
149void 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
223void 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}
The exception type for ompl.
Definition: Exception.h:47
virtual void setup()
Perform additional setup tasks (run once, before use). If state validity checking resolution has not ...
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::StatePropagator.
Definition of an abstract state.
Definition: State.h:50
A shared pointer wrapper for ompl::control::ControlSpace.
Definition of an abstract control.
Definition: Control.h:48
A shared pointer wrapper for ompl::control::DirectedControlSampler.
Space information containing necessary information for planning with controls. setup() needs to be ca...
SpaceInformation(const base::StateSpacePtr &stateSpace, ControlSpacePtr controlSpace)
Constructor. Sets the instance of the state and control spaces to plan with.
DirectedControlSamplerPtr allocDirectedControlSampler() const
Allocate an instance of the DirectedControlSampler to use. This will be the default (SimpleDirectedCo...
void printSettings(std::ostream &out=std::cout) const override
Print information about the current instance of the state space.
void setStatePropagator(const StatePropagatorFn &fn)
Set the function that performs state propagation.
void setup() override
Perform additional setup tasks (run once, before use)
void setDirectedControlSamplerAllocator(const DirectedControlSamplerAllocator &dcsa)
Set the allocator to use for the DirectedControlSampler.
bool canPropagateBackward() const
Some systems can only propagate forward in time (i.e., the steps argument for the propagate() functio...
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,...
void clearDirectedSamplerAllocator()
Reset the DirectedControlSampler to be the default one.
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,...
Model the effect of controls on system states.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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.
STL namespace.