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/base/SpaceInformation.h"
00038 #include "ompl/base/samplers/UniformValidStateSampler.h"
00039 #include "ompl/base/DiscreteMotionValidator.h"
00040 #include "ompl/util/Exception.h"
00041 #include <queue>
00042 #include <cassert>
00043
00044 ompl::base::SpaceInformation::SpaceInformation(const StateManifoldPtr &manifold) :
00045 stateManifold_(manifold), motionValidator_(new DiscreteMotionValidator(this)), setup_(false), msg_("SpaceInformation")
00046 {
00047 if (!stateManifold_)
00048 throw Exception("Invalid manifold definition");
00049 }
00050
00051 void ompl::base::SpaceInformation::setup(void)
00052 {
00053 if (!stateValidityChecker_)
00054 {
00055 stateValidityChecker_.reset(new AllValidStateValidityChecker(this));
00056 msg_.warn("State validity checker not set! No collision checking is performed");
00057 }
00058
00059 if (!motionValidator_)
00060 motionValidator_.reset(new DiscreteMotionValidator(this));
00061
00062 stateManifold_->setup();
00063 if (stateManifold_->getDimension() <= 0)
00064 throw Exception("The dimension of the state manifold we plan in must be > 0");
00065
00066 setup_ = true;
00067 }
00068
00069 bool ompl::base::SpaceInformation::isSetup(void) const
00070 {
00071 return setup_;
00072 }
00073
00074 void ompl::base::SpaceInformation::setStateValidityChecker(const StateValidityCheckerFn &svc)
00075 {
00076 class BoostFnStateValidityChecker : public StateValidityChecker
00077 {
00078 public:
00079
00080 BoostFnStateValidityChecker(SpaceInformation* si,
00081 const StateValidityCheckerFn &fn) : StateValidityChecker(si), fn_(fn)
00082 {
00083 }
00084
00085 virtual bool isValid(const State *state) const
00086 {
00087 return fn_(state);
00088 }
00089
00090 protected:
00091
00092 StateValidityCheckerFn fn_;
00093 };
00094
00095 if (!svc)
00096 throw Exception("Invalid function definition for state validity checking");
00097
00098 setStateValidityChecker(StateValidityCheckerPtr(dynamic_cast<StateValidityChecker*>(new BoostFnStateValidityChecker(this, svc))));
00099 }
00100
00101 unsigned int ompl::base::SpaceInformation::randomBounceMotion(const ManifoldStateSamplerPtr &mss, const State *start, unsigned int steps, std::vector<State*> &states, bool alloc) const
00102 {
00103 if (alloc)
00104 {
00105 states.resize(steps);
00106 for (unsigned int i = 0 ; i < steps ; ++i)
00107 states[i] = allocState();
00108 }
00109 else
00110 if (states.size() < steps)
00111 steps = states.size();
00112
00113 const State *prev = start;
00114 std::pair<State*, double> lastValid;
00115
00116 unsigned int j = 0;
00117 for (unsigned int i = 0 ; i < steps ; ++i)
00118 {
00119 mss->sampleUniform(states[j]);
00120 lastValid.first = states[j];
00121 if (checkMotion(prev, states[j], lastValid) || lastValid.second > std::numeric_limits<double>::epsilon())
00122 prev = states[j++];
00123 }
00124 return j;
00125 }
00126
00127 bool ompl::base::SpaceInformation::searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near, double distance) const
00128 {
00129 if (state != near)
00130 copyState(state, near);
00131
00132
00133 if (!satisfiesBounds(state))
00134 enforceBounds(state);
00135
00136 bool result = isValid(state);
00137
00138 if (!result)
00139 {
00140
00141 State *temp = cloneState(state);
00142 result = sampler->sampleNear(state, temp, distance);
00143 freeState(temp);
00144 }
00145
00146 return result;
00147 }
00148
00149 bool ompl::base::SpaceInformation::searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const
00150 {
00151 if (satisfiesBounds(near) && isValid(near))
00152 {
00153 if (state != near)
00154 copyState(state, near);
00155 return true;
00156 }
00157 else
00158 {
00159
00160 UniformValidStateSampler *uvss = new UniformValidStateSampler(this);
00161 uvss->setNrAttempts(attempts);
00162 return searchValidNearby(ValidStateSamplerPtr(uvss), state, near, distance);
00163 }
00164 }
00165
00166 unsigned int ompl::base::SpaceInformation::getMotionStates(const State *s1, const State *s2, std::vector<State*> &states, unsigned int count, bool endpoints, bool alloc) const
00167 {
00168
00169 count++;
00170
00171 if (count < 2)
00172 {
00173 unsigned int added = 0;
00174
00175
00176 if (endpoints)
00177 {
00178 if (alloc)
00179 {
00180 states.resize(2);
00181 states[0] = allocState();
00182 states[1] = allocState();
00183 }
00184 if (states.size() > 0)
00185 {
00186 copyState(states[0], s1);
00187 added++;
00188 }
00189
00190 if (states.size() > 1)
00191 {
00192 copyState(states[1], s2);
00193 added++;
00194 }
00195 }
00196 else
00197 if (alloc)
00198 states.resize(0);
00199 return added;
00200 }
00201
00202 if (alloc)
00203 {
00204 states.resize(count + (endpoints ? 1 : -1));
00205 if (endpoints)
00206 states[0] = allocState();
00207 }
00208
00209 unsigned int added = 0;
00210
00211 if (endpoints && states.size() > 0)
00212 {
00213 copyState(states[0], s1);
00214 added++;
00215 }
00216
00217
00218 for (unsigned int j = 1 ; j < count && added < states.size() ; ++j)
00219 {
00220 if (alloc)
00221 states[added] = allocState();
00222 stateManifold_->interpolate(s1, s2, (double)j / (double)count, states[added]);
00223 added++;
00224 }
00225
00226 if (added < states.size() && endpoints)
00227 {
00228 if (alloc)
00229 states[added] = allocState();
00230 copyState(states[added], s2);
00231 added++;
00232 }
00233
00234 return added;
00235 }
00236
00237
00238 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State*> &states, unsigned int count, unsigned int &firstInvalidStateIndex) const
00239 {
00240 assert(states.size() >= count);
00241 for (unsigned int i = 0 ; i < count ; ++i)
00242 if (!isValid(states[i]))
00243 {
00244 firstInvalidStateIndex = i;
00245 return false;
00246 }
00247 return true;
00248 }
00249
00250 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State*> &states, unsigned int count) const
00251 {
00252 assert(states.size() >= count);
00253 if (count > 0)
00254 {
00255 if (count > 1)
00256 {
00257 if (!isValid(states.front()))
00258 return false;
00259 if (!isValid(states[count - 1]))
00260 return false;
00261
00262
00263
00264 if (count > 2)
00265 {
00266 std::queue< std::pair<int, int> > pos;
00267 pos.push(std::make_pair(0, count - 1));
00268
00269 while (!pos.empty())
00270 {
00271 std::pair<int, int> x = pos.front();
00272
00273 int mid = (x.first + x.second) / 2;
00274 if (!isValid(states[mid]))
00275 return false;
00276
00277 if (x.first < mid - 1)
00278 pos.push(std::make_pair(x.first, mid));
00279 if (x.second > mid + 1)
00280 pos.push(std::make_pair(mid, x.second));
00281 }
00282 }
00283 }
00284 else
00285 return isValid(states.front());
00286 }
00287 return true;
00288 }
00289
00290 ompl::base::ValidStateSamplerPtr ompl::base::SpaceInformation::allocValidStateSampler(void) const
00291 {
00292 if (vssa_)
00293 return vssa_(this);
00294 else
00295 return ValidStateSamplerPtr(new UniformValidStateSampler(this));
00296 }
00297
00298 void ompl::base::SpaceInformation::printSettings(std::ostream &out) const
00299 {
00300 out << "State space settings:" << std::endl;
00301 out << " - dimension: " << stateManifold_->getDimension() << std::endl;
00302 out << " - extent: " << stateManifold_->getMaximumExtent() << std::endl;
00303 out << " - state validity check resolution: " << (getStateValidityCheckingResolution() * 100.0) << '%' << std::endl;
00304 out << " - valid segment count factor: " << stateManifold_->getValidSegmentCountFactor() << std::endl;
00305 out << " - state manifold:" << std::endl;
00306 stateManifold_->printSettings(out);
00307 }