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/ProblemDefinition.h"
00038 #include "ompl/base/goals/GoalState.h"
00039 #include "ompl/base/goals/GoalStates.h"
00040 #include "ompl/control/SpaceInformation.h"
00041 #include "ompl/control/PathControl.h"
00042 #include <sstream>
00043 #include <algorithm>
00044
00045 #include <boost/thread/mutex.hpp>
00046
00048 namespace ompl
00049 {
00050 namespace base
00051 {
00052
00053 class ProblemDefinition::PlannerSolutionSet
00054 {
00055 public:
00056
00057 PlannerSolutionSet(void)
00058 {
00059 }
00060
00061 void add(const PlannerSolution &s)
00062 {
00063 boost::mutex::scoped_lock slock(lock_);
00064 int index = solutions_.size();
00065 solutions_.push_back(s);
00066 solutions_.back().index_ = index;
00067 std::sort(solutions_.begin(), solutions_.end());
00068 }
00069
00070 void clear(void)
00071 {
00072 boost::mutex::scoped_lock slock(lock_);
00073 solutions_.clear();
00074 }
00075
00076 std::vector<PlannerSolution> getSolutions(void)
00077 {
00078 boost::mutex::scoped_lock slock(lock_);
00079 std::vector<PlannerSolution> copy = solutions_;
00080 return copy;
00081 }
00082
00083 bool isApproximate(void)
00084 {
00085 boost::mutex::scoped_lock slock(lock_);
00086 bool result = false;
00087 if (!solutions_.empty())
00088 result = solutions_[0].approximate_;
00089 return result;
00090 }
00091
00092 double getDifference(void)
00093 {
00094 boost::mutex::scoped_lock slock(lock_);
00095 double diff = -1.0;
00096 if (!solutions_.empty())
00097 diff = solutions_[0].difference_;
00098 return diff;
00099 }
00100
00101 PathPtr getTopSolution(void)
00102 {
00103 boost::mutex::scoped_lock slock(lock_);
00104 PathPtr copy;
00105 if (!solutions_.empty())
00106 copy = solutions_[0].path_;
00107 return copy;
00108 }
00109
00110 std::size_t getSolutionCount(void)
00111 {
00112 boost::mutex::scoped_lock slock(lock_);
00113 std::size_t result = solutions_.size();
00114 return result;
00115 }
00116
00117 private:
00118
00119 std::vector<PlannerSolution> solutions_;
00120 boost::mutex lock_;
00121 };
00122 }
00123 }
00125
00126 ompl::base::ProblemDefinition::ProblemDefinition(const SpaceInformationPtr &si) : si_(si), solutions_(new PlannerSolutionSet())
00127 {
00128 }
00129
00130 void ompl::base::ProblemDefinition::setStartAndGoalStates(const State *start, const State *goal, const double threshold)
00131 {
00132 clearStartStates();
00133 addStartState(start);
00134 setGoalState(goal, threshold);
00135 }
00136
00137 void ompl::base::ProblemDefinition::setGoalState(const State *goal, const double threshold)
00138 {
00139 clearGoal();
00140 GoalState *gs = new GoalState(si_);
00141 gs->setState(goal);
00142 gs->setThreshold(threshold);
00143 setGoal(GoalPtr(gs));
00144 }
00145
00146 bool ompl::base::ProblemDefinition::hasStartState(const State *state, unsigned int *startIndex)
00147 {
00148 for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00149 if (si_->equalStates(state, startStates_[i]))
00150 {
00151 if (startIndex)
00152 *startIndex = i;
00153 return true;
00154 }
00155 return false;
00156 }
00157
00158 bool ompl::base::ProblemDefinition::fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
00159 {
00160 bool result = false;
00161
00162 bool b = si_->satisfiesBounds(state);
00163 bool v = false;
00164 if (b)
00165 {
00166 v = si_->isValid(state);
00167 if (!v)
00168 logDebug("%s state is not valid", start ? "Start" : "Goal");
00169 }
00170 else
00171 logDebug("%s state is not within space bounds", start ? "Start" : "Goal");
00172
00173 if (!b || !v)
00174 {
00175 std::stringstream ss;
00176 si_->printState(state, ss);
00177 ss << " within distance " << dist;
00178 logDebug("Attempting to fix %s state %s", start ? "start" : "goal", ss.str().c_str());
00179
00180 State *temp = si_->allocState();
00181 if (si_->searchValidNearby(temp, state, dist, attempts))
00182 {
00183 si_->copyState(state, temp);
00184 result = true;
00185 }
00186 else
00187 logWarn("Unable to fix %s state", start ? "start" : "goal");
00188 si_->freeState(temp);
00189 }
00190
00191 return result;
00192 }
00193
00194 bool ompl::base::ProblemDefinition::fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts)
00195 {
00196 bool result = true;
00197
00198
00199 for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00200 if (!fixInvalidInputState(startStates_[i], distStart, true, attempts))
00201 result = false;
00202
00203
00204 GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
00205 if (goal)
00206 {
00207 if (!fixInvalidInputState(const_cast<State*>(goal->getState()), distGoal, false, attempts))
00208 result = false;
00209 }
00210
00211
00212 GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
00213 if (goals)
00214 {
00215 for (unsigned int i = 0; i < goals->getStateCount(); ++i)
00216 if (!fixInvalidInputState(const_cast<State*>(goals->getState(i)), distGoal, false, attempts))
00217 result = false;
00218 }
00219
00220 return result;
00221 }
00222
00223 void ompl::base::ProblemDefinition::getInputStates(std::vector<const State*> &states) const
00224 {
00225 states.clear();
00226 for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00227 states.push_back(startStates_[i]);
00228
00229 GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
00230 if (goal)
00231 states.push_back(goal->getState());
00232
00233 GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
00234 if (goals)
00235 for (unsigned int i = 0; i < goals->getStateCount(); ++i)
00236 states.push_back (goals->getState(i));
00237 }
00238
00239 ompl::base::PathPtr ompl::base::ProblemDefinition::isStraightLinePathValid(void) const
00240 {
00241 PathPtr path;
00242 if (control::SpaceInformationPtr sic = boost::dynamic_pointer_cast<control::SpaceInformation, SpaceInformation>(si_))
00243 {
00244 unsigned int startIndex;
00245 if (isTrivial(&startIndex, NULL))
00246 {
00247 control::PathControl *pc = new control::PathControl(sic);
00248 pc->append(startStates_[startIndex]);
00249 control::Control *null = sic->allocControl();
00250 sic->nullControl(null);
00251 pc->append(startStates_[startIndex], null, 0.0);
00252 sic->freeControl(null);
00253 path.reset(pc);
00254 }
00255 else
00256 {
00257 control::Control *nc = sic->allocControl();
00258 State *result1 = sic->allocState();
00259 State *result2 = sic->allocState();
00260 sic->nullControl(nc);
00261
00262 for (unsigned int k = 0 ; k < startStates_.size() && !path ; ++k)
00263 {
00264 const State *start = startStates_[k];
00265 if (start && si_->isValid(start) && si_->satisfiesBounds(start))
00266 {
00267 sic->copyState(result1, start);
00268 for (unsigned int i = 0 ; i < sic->getMaxControlDuration() && !path ; ++i)
00269 if (sic->propagateWhileValid(result1, nc, 1, result2))
00270 {
00271 if (goal_->isSatisfied(result2))
00272 {
00273 control::PathControl *pc = new control::PathControl(sic);
00274 pc->append(start);
00275 pc->append(result2, nc, (i + 1) * sic->getPropagationStepSize());
00276 path.reset(pc);
00277 break;
00278 }
00279 std::swap(result1, result2);
00280 }
00281 }
00282 }
00283 sic->freeState(result1);
00284 sic->freeState(result2);
00285 sic->freeControl(nc);
00286 }
00287 }
00288 else
00289 {
00290 std::vector<const State*> states;
00291 GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
00292 if (goal)
00293 if (si_->isValid(goal->getState()) && si_->satisfiesBounds(goal->getState()))
00294 states.push_back(goal->getState());
00295 GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
00296 if (goals)
00297 for (unsigned int i = 0; i < goals->getStateCount(); ++i)
00298 if (si_->isValid(goals->getState(i)) && si_->satisfiesBounds(goals->getState(i)))
00299 states.push_back(goals->getState(i));
00300
00301 if (states.empty())
00302 {
00303 unsigned int startIndex;
00304 if (isTrivial(&startIndex))
00305 {
00306 geometric::PathGeometric *pg = new geometric::PathGeometric(si_, startStates_[startIndex], startStates_[startIndex]);
00307 path.reset(pg);
00308 }
00309 }
00310 else
00311 {
00312 for (unsigned int i = 0 ; i < startStates_.size() && !path ; ++i)
00313 {
00314 const State *start = startStates_[i];
00315 if (start && si_->isValid(start) && si_->satisfiesBounds(start))
00316 {
00317 for (unsigned int j = 0 ; j < states.size() && !path ; ++j)
00318 if (si_->checkMotion(start, states[j]))
00319 {
00320 geometric::PathGeometric *pg = new geometric::PathGeometric(si_, start, states[j]);
00321 path.reset(pg);
00322 break;
00323 }
00324 }
00325 }
00326 }
00327 }
00328
00329 return path;
00330 }
00331
00332 bool ompl::base::ProblemDefinition::isTrivial(unsigned int *startIndex, double *distance) const
00333 {
00334 if (!goal_)
00335 {
00336 logError("Goal undefined");
00337 return false;
00338 }
00339
00340 for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00341 {
00342 const State *start = startStates_[i];
00343 if (start && si_->isValid(start) && si_->satisfiesBounds(start))
00344 {
00345 double dist;
00346 if (goal_->isSatisfied(start, &dist))
00347 {
00348 if (startIndex)
00349 *startIndex = i;
00350 if (distance)
00351 *distance = dist;
00352 return true;
00353 }
00354 }
00355 else
00356 {
00357 logError("Initial state is in collision!");
00358 }
00359 }
00360
00361 return false;
00362 }
00363
00364 bool ompl::base::ProblemDefinition::hasSolution(void) const
00365 {
00366 return solutions_->getSolutionCount() > 0;
00367 }
00368
00369 std::size_t ompl::base::ProblemDefinition::getSolutionCount(void) const
00370 {
00371 return solutions_->getSolutionCount();
00372 }
00373
00374 ompl::base::PathPtr ompl::base::ProblemDefinition::getSolutionPath(void) const
00375 {
00376 return solutions_->getTopSolution();
00377 }
00378
00379 void ompl::base::ProblemDefinition::addSolutionPath(const PathPtr &path, bool approximate, double difference) const
00380 {
00381 if (approximate)
00382 logWarn("Adding approximate solution");
00383 solutions_->add(PlannerSolution(path, approximate, difference));
00384 }
00385
00386 bool ompl::base::ProblemDefinition::hasApproximateSolution(void) const
00387 {
00388 return solutions_->isApproximate();
00389 }
00390
00391 double ompl::base::ProblemDefinition::getSolutionDifference(void) const
00392 {
00393 return solutions_->getDifference();
00394 }
00395
00396 std::vector<ompl::base::PlannerSolution> ompl::base::ProblemDefinition::getSolutions(void) const
00397 {
00398 return solutions_->getSolutions();
00399 }
00400
00401 void ompl::base::ProblemDefinition::clearSolutionPaths(void) const
00402 {
00403 solutions_->clear();
00404 }
00405
00406 void ompl::base::ProblemDefinition::print(std::ostream &out) const
00407 {
00408 out << "Start states:" << std::endl;
00409 for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00410 si_->printState(startStates_[i], out);
00411 if (goal_)
00412 goal_->print(out);
00413 else
00414 out << "Goal = NULL" << std::endl;
00415 out << "There are " << solutions_->getSolutionCount() << " solutions" << std::endl;
00416 }
00417
00418
00419 bool ompl::base::ProblemDefinition::hasSolutionNonExistenceProof(void) const
00420 {
00421 return nonExistenceProof_;
00422 }
00423
00424 void ompl::base::ProblemDefinition::clearSolutionNonExistenceProof(void)
00425 {
00426 nonExistenceProof_.reset();
00427 }
00428
00429 const ompl::base::SolutionNonExistenceProofPtr& ompl::base::ProblemDefinition::getSolutionNonExistenceProof(void) const
00430 {
00431 return nonExistenceProof_;
00432 }
00433
00434 void ompl::base::ProblemDefinition::setSolutionNonExistenceProof(const ompl::base::SolutionNonExistenceProofPtr& nonExistenceProof)
00435 {
00436 nonExistenceProof_ = nonExistenceProof;
00437 }