37 #include "ompl/control/SpaceInformation.h"
38 #include "ompl/control/SimpleDirectedControlSampler.h"
39 #include "ompl/util/Exception.h"
48 throw Exception(
"State propagator not defined");
50 throw Exception(
"The minimum number of steps cannot be larger than the maximum number of steps");
58 throw Exception(
"The minimum number of steps must be at least 1");
60 if (
stepSize_ < std::numeric_limits<double>::epsilon())
63 if (
stepSize_ < std::numeric_limits<double>::epsilon())
64 throw Exception(
"The propagation step size must be larger than 0");
70 throw Exception(
"The dimension of the control space we plan in must be > 0");
105 fn_(state, control, duration, result);
114 setStatePropagator(StatePropagatorPtr(dynamic_cast<StatePropagator*>(
new BoostFnStatePropagator(
this, fn))));
119 statePropagator_ = sp;
124 return statePropagator_->canPropagateBackward();
132 copyState(result, state);
136 double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
139 statePropagator_->propagate(state, control, signedStepSize, result);
140 for (
int i = 1 ; i < steps ; ++i)
141 statePropagator_->propagate(result, control, signedStepSize, result);
150 copyState(result, state);
154 double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
158 statePropagator_->propagate(state, control, signedStepSize, result);
166 unsigned int r = steps;
169 for (
int i = 1 ; i < steps ; ++i)
171 statePropagator_->propagate(temp1, control, signedStepSize, temp2);
173 std::swap(temp1, temp2);
185 copyState(result, temp1);
197 copyState(result, state);
204 double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
209 result.resize(steps);
210 for (
unsigned int i = 0 ; i < result.size() ; ++i)
211 result[i] = allocState();
217 steps = std::min(steps, (
int)result.size());
224 statePropagator_->propagate(state, control, signedStepSize, result[st]);
229 statePropagator_->propagate(result[st-1], control, signedStepSize, result[st]);
237 double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
241 result.resize(steps);
246 steps = std::min(steps, (
int)result.size());
254 result[st] = allocState();
255 statePropagator_->propagate(state, control, signedStepSize, result[st]);
257 if (isValid(result[st]))
263 result[st] = allocState();
264 statePropagator_->propagate(result[st-1], control, signedStepSize, result[st]);
266 if (!isValid(result[st]))
270 freeState(result[st]);
283 freeState(result[st]);
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;
boost::function< void(const base::State *, const Control *, const double, base::State *)> StatePropagatorFn
A function that achieves state propagation.
Definition of an abstract control.
Model the effect of controls on system states.
boost::function< DirectedControlSamplerPtr(const SpaceInformation *)> DirectedControlSamplerAllocator
Definition of a function that can allocate a directed control sampler.
Implementation of a simple directed control sampler. This is a basic implementation that does not act...
A boost shared pointer wrapper for ompl::control::DirectedControlSampler.
Definition of an abstract state.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
The exception type for ompl.