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