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
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 {
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
95ompl::control::DirectedControlSamplerPtr ompl::control::SpaceInformation::allocDirectedControlSampler() const
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
110
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
143
145{
146 return statePropagator_->canPropagateBackward();
147}
148
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
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
void freeState(State *state) const
Free the memory of a state.
virtual void setup()
Perform additional setup tasks (run once, before use). If state validity checking resolution has not ...
double getStateValidityCheckingResolution() const
Get the resolution at which state validity is verified. This call is only applicable if a ompl::base:...
ParamSet params_
Combined parameters for the contained classes.
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.
double getMaximumExtent() const
Get the maximum extent of the space we are planning in. This is the maximum distance that could be re...
State * allocState() const
Allocate memory for a state.
bool setup_
Flag indicating whether setup() has been called on this instance.
bool isValid(const State *state) const
Check if a given state is valid or not.
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
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.
unsigned int minSteps_
The minimum number of steps to apply a control for.
DirectedControlSamplerAllocator dcsa_
Optional allocator for the DirectedControlSampler. If not specified, the default implementation is us...
unsigned int getMinControlDuration() const
Get the minimum number of steps a control is propagated for.
void setStatePropagator(const StatePropagatorFn &fn)
Set the function that performs state propagation.
double getPropagationStepSize() const
Propagation is performed at integer multiples of a specified step size. This function returns the val...
void setup() override
Perform additional setup tasks (run once, before use)
void setPropagationStepSize(double stepSize)
When controls are applied to states, they are applied for a time duration that is an integer multiple...
ControlSpacePtr controlSpace_
The control space describing the space of controls applicable to states in the state space.
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...
unsigned int maxSteps_
The maximum number of steps to apply a control for.
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 setMaxControlDuration(unsigned int maxSteps)
Set the minimum and maximum number of steps a control is propagated for.
void clearDirectedSamplerAllocator()
Reset the DirectedControlSampler to be the default one.
unsigned int getMaxControlDuration() const
Get the maximum number of steps a control is propagated for.
StatePropagatorPtr statePropagator_
The state propagator used to model the motion of the system being planned for.
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,...
void setMinControlDuration(unsigned int minSteps)
Set the minimum number of steps a control is propagated for.
Model the effect of controls on system states.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition Control.h:45
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.