00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/control/SpaceInformation.h"
00038 #include "ompl/control/SimpleDirectedControlSampler.h"
00039 #include "ompl/util/Exception.h"
00040 #include <cassert>
00041 #include <utility>
00042 #include <limits>
00043
00044 void ompl::control::SpaceInformation::setup(void)
00045 {
00046 base::SpaceInformation::setup();
00047 if (!statePropagator_)
00048 throw Exception("State propagator not defined");
00049 if (minSteps_ > maxSteps_)
00050 throw Exception("The minimum number of steps cannot be larger than the maximum number of steps");
00051 if (minSteps_ == 0 && maxSteps_ == 0)
00052 {
00053 minSteps_ = 1;
00054 maxSteps_ = 10;
00055 msg_.warn("Assuming propagation will always have between %d and %d steps", minSteps_, maxSteps_);
00056 }
00057 if (minSteps_ < 1)
00058 throw Exception("The minimum number of steps must be at least 1");
00059
00060 if (stepSize_ < std::numeric_limits<double>::epsilon())
00061 {
00062 stepSize_ = getStateValidityCheckingResolution() * getMaximumExtent();
00063 if (stepSize_ < std::numeric_limits<double>::epsilon())
00064 throw Exception("The propagation step size must be larger than 0");
00065 msg_.warn("The propagation step size is assumed to be %f", stepSize_);
00066 }
00067
00068 controlSpace_->setup();
00069 if (controlSpace_->getDimension() <= 0)
00070 throw Exception("The dimension of the control space we plan in must be > 0");
00071 }
00072
00073 ompl::control::DirectedControlSamplerPtr ompl::control::SpaceInformation::allocDirectedControlSampler(void) const
00074 {
00075 if (dcsa_)
00076 return dcsa_(this);
00077 else
00078 return DirectedControlSamplerPtr(new SimpleDirectedControlSampler(this));
00079 }
00080
00081 void ompl::control::SpaceInformation::setDirectedControlSamplerAllocator(const DirectedControlSamplerAllocator &dcsa)
00082 {
00083 dcsa_ = dcsa;
00084 setup_ = false;
00085 }
00086
00087 void ompl::control::SpaceInformation::clearDirectedSamplerAllocator(void)
00088 {
00089 dcsa_ = DirectedControlSamplerAllocator();
00090 setup_ = false;
00091 }
00092
00093 void ompl::control::SpaceInformation::setStatePropagator(const StatePropagatorFn &fn)
00094 {
00095 class BoostFnStatePropagator : public StatePropagator
00096 {
00097 public:
00098
00099 BoostFnStatePropagator(SpaceInformation *si, const StatePropagatorFn &fn) : StatePropagator(si), fn_(fn)
00100 {
00101 }
00102
00103 virtual void propagate(const base::State *state, const Control* control, const double duration, base::State *result) const
00104 {
00105 fn_(state, control, duration, result);
00106 }
00107
00108 protected:
00109
00110 StatePropagatorFn fn_;
00111
00112 };
00113
00114 setStatePropagator(StatePropagatorPtr(dynamic_cast<StatePropagator*>(new BoostFnStatePropagator(this, fn))));
00115 }
00116
00117 void ompl::control::SpaceInformation::setStatePropagator(const StatePropagatorPtr &sp)
00118 {
00119 statePropagator_ = sp;
00120 }
00121
00122 bool ompl::control::SpaceInformation::canPropagateBackward(void) const
00123 {
00124 return statePropagator_->canPropagateBackward();
00125 }
00126
00127 void ompl::control::SpaceInformation::propagate(const base::State *state, const Control* control, int steps, base::State *result) const
00128 {
00129 if (steps == 0)
00130 {
00131 if (result != state)
00132 copyState(result, state);
00133 }
00134 else
00135 {
00136 double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
00137 steps = abs(steps);
00138
00139 statePropagator_->propagate(state, control, signedStepSize, result);
00140 for (int i = 1 ; i < steps ; ++i)
00141 statePropagator_->propagate(result, control, signedStepSize, result);
00142 }
00143 }
00144
00145 unsigned int ompl::control::SpaceInformation::propagateWhileValid(const base::State *state, const Control* control, int steps, base::State *result) const
00146 {
00147 if (steps == 0)
00148 {
00149 if (result != state)
00150 copyState(result, state);
00151 return 0;
00152 }
00153
00154 double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
00155 steps = abs(steps);
00156
00157
00158 statePropagator_->propagate(state, control, signedStepSize, result);
00159
00160
00161 if (isValid(result))
00162 {
00163 base::State *temp1 = result;
00164 base::State *temp2 = allocState();
00165 base::State *toDelete = temp2;
00166 unsigned int r = steps;
00167
00168
00169 for (int i = 1 ; i < steps ; ++i)
00170 {
00171 statePropagator_->propagate(temp1, control, signedStepSize, temp2);
00172 if (isValid(temp2))
00173 std::swap(temp1, temp2);
00174 else
00175 {
00176
00177 r = i;
00178 break;
00179 }
00180 }
00181
00182
00183
00184 if (result != temp1)
00185 copyState(result, temp1);
00186
00187
00188 freeState(toDelete);
00189
00190 return r;
00191 }
00192
00193
00194 else
00195 {
00196 if (result != state)
00197 copyState(result, state);
00198 return 0;
00199 }
00200 }
00201
00202 void ompl::control::SpaceInformation::propagate(const base::State *state, const Control* control, int steps, std::vector<base::State*> &result, bool alloc) const
00203 {
00204 double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
00205 steps = abs(steps);
00206
00207 if (alloc)
00208 {
00209 result.resize(steps);
00210 for (unsigned int i = 0 ; i < result.size() ; ++i)
00211 result[i] = allocState();
00212 }
00213 else
00214 {
00215 if (result.empty())
00216 return;
00217 steps = std::min(steps, (int)result.size());
00218 }
00219
00220 int st = 0;
00221
00222 if (st < steps)
00223 {
00224 statePropagator_->propagate(state, control, signedStepSize, result[st]);
00225 ++st;
00226
00227 while (st < steps)
00228 {
00229 statePropagator_->propagate(result[st-1], control, signedStepSize, result[st]);
00230 ++st;
00231 }
00232 }
00233 }
00234
00235 unsigned int ompl::control::SpaceInformation::propagateWhileValid(const base::State *state, const Control* control, int steps, std::vector<base::State*> &result, bool alloc) const
00236 {
00237 double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
00238 steps = abs(steps);
00239
00240 if (alloc)
00241 result.resize(steps);
00242 else
00243 {
00244 if (result.empty())
00245 return 0;
00246 steps = std::min(steps, (int)result.size());
00247 }
00248
00249 int st = 0;
00250
00251 if (st < steps)
00252 {
00253 if (alloc)
00254 result[st] = allocState();
00255 statePropagator_->propagate(state, control, signedStepSize, result[st]);
00256
00257 if (isValid(result[st]))
00258 {
00259 ++st;
00260 while (st < steps)
00261 {
00262 if (alloc)
00263 result[st] = allocState();
00264 statePropagator_->propagate(result[st-1], control, signedStepSize, result[st]);
00265
00266 if (!isValid(result[st]))
00267 {
00268 if (alloc)
00269 {
00270 freeState(result[st]);
00271 result.resize(st);
00272 }
00273 break;
00274 }
00275 else
00276 ++st;
00277 }
00278 }
00279 else
00280 {
00281 if (alloc)
00282 {
00283 freeState(result[st]);
00284 result.resize(st);
00285 }
00286 }
00287 }
00288
00289 return st;
00290 }
00291
00292 void ompl::control::SpaceInformation::printSettings(std::ostream &out) const
00293 {
00294 base::SpaceInformation::printSettings(out);
00295 out << " - control space:" << std::endl;
00296 controlSpace_->printSettings(out);
00297 out << " - can propagate backward: " << (canPropagateBackward() ? "yes" : "no") << std::endl;
00298 out << " - propagation step size: " << stepSize_ << std::endl;
00299 out << " - propagation duration: [" << minSteps_ << ", " << maxSteps_ << "]" << std::endl;
00300 }