37 #include "ompl/base/ProblemDefinition.h"
38 #include "ompl/base/goals/GoalState.h"
39 #include "ompl/base/goals/GoalStates.h"
40 #include "ompl/base/OptimizationObjective.h"
41 #include "ompl/control/SpaceInformation.h"
42 #include "ompl/control/PathControl.h"
43 #include "ompl/tools/config/MagicConstants.h"
47 #include <boost/thread/mutex.hpp>
55 class ProblemDefinition::PlannerSolutionSet
59 PlannerSolutionSet(
void)
63 void add(
const PlannerSolution &s)
65 boost::mutex::scoped_lock slock(lock_);
66 int index = solutions_.size();
67 solutions_.push_back(s);
68 solutions_.back().index_ = index;
69 std::sort(solutions_.begin(), solutions_.end());
74 boost::mutex::scoped_lock slock(lock_);
80 boost::mutex::scoped_lock slock(lock_);
81 std::vector<PlannerSolution> copy = solutions_;
85 bool isApproximate(
void)
87 boost::mutex::scoped_lock slock(lock_);
89 if (!solutions_.empty())
90 result = solutions_[0].approximate_;
94 bool isOptimized(
void)
96 boost::mutex::scoped_lock slock(lock_);
98 if (!solutions_.empty())
99 result = solutions_[0].optimized_;
103 double getDifference(
void)
105 boost::mutex::scoped_lock slock(lock_);
107 if (!solutions_.empty())
108 diff = solutions_[0].difference_;
112 PathPtr getTopSolution(
void)
114 boost::mutex::scoped_lock slock(lock_);
116 if (!solutions_.empty())
117 copy = solutions_[0].path_;
123 boost::mutex::scoped_lock slock(lock_);
124 std::size_t result = solutions_.size();
130 std::vector<PlannerSolution> solutions_;
144 addStartState(start);
145 setGoalState(goal, threshold);
159 for (
unsigned int i = 0 ; i < startStates_.size() ; ++i)
160 if (si_->equalStates(state, startStates_[i]))
173 bool b = si_->satisfiesBounds(state);
177 v = si_->isValid(state);
179 OMPL_DEBUG(
"%s state is not valid", start ?
"Start" :
"Goal");
182 OMPL_DEBUG(
"%s state is not within space bounds", start ?
"Start" :
"Goal");
186 std::stringstream ss;
187 si_->printState(state, ss);
188 ss <<
" within distance " << dist;
189 OMPL_DEBUG(
"Attempting to fix %s state %s", start ?
"start" :
"goal", ss.str().c_str());
191 State *temp = si_->allocState();
192 if (si_->searchValidNearby(temp, state, dist, attempts))
194 si_->copyState(state, temp);
198 OMPL_WARN(
"Unable to fix %s state", start ?
"start" :
"goal");
199 si_->freeState(temp);
210 for (
unsigned int i = 0 ; i < startStates_.size() ; ++i)
211 if (!fixInvalidInputState(startStates_[i], distStart,
true, attempts))
218 if (!fixInvalidInputState(const_cast<State*>(goal->
getState()), distGoal,
false, attempts))
227 if (!fixInvalidInputState(const_cast<State*>(goals->
getState(i)), distGoal,
false, attempts))
237 for (
unsigned int i = 0 ; i < startStates_.size() ; ++i)
238 states.push_back(startStates_[i]);
247 states.push_back (goals->
getState(i));
255 unsigned int startIndex;
256 if (isTrivial(&startIndex, NULL))
259 pc->
append(startStates_[startIndex]);
261 sic->nullControl(null);
262 pc->
append(startStates_[startIndex], null, 0.0);
263 sic->freeControl(null);
269 State *result1 = sic->allocState();
270 State *result2 = sic->allocState();
271 sic->nullControl(nc);
273 for (
unsigned int k = 0 ; k < startStates_.size() && !path ; ++k)
275 const State *start = startStates_[k];
276 if (start && si_->isValid(start) && si_->satisfiesBounds(start))
278 sic->copyState(result1, start);
279 for (
unsigned int i = 0 ; i < sic->getMaxControlDuration() && !path ; ++i)
280 if (sic->propagateWhileValid(result1, nc, 1, result2))
282 if (goal_->isSatisfied(result2))
286 pc->
append(result2, nc, (i + 1) * sic->getPropagationStepSize());
290 std::swap(result1, result2);
294 sic->freeState(result1);
295 sic->freeState(result2);
296 sic->freeControl(nc);
301 std::vector<const State*> states;
304 if (si_->isValid(goal->
getState()) && si_->satisfiesBounds(goal->
getState()))
309 if (si_->isValid(goals->
getState(i)) && si_->satisfiesBounds(goals->
getState(i)))
310 states.push_back(goals->
getState(i));
314 unsigned int startIndex;
315 if (isTrivial(&startIndex))
323 for (
unsigned int i = 0 ; i < startStates_.size() && !path ; ++i)
325 const State *start = startStates_[i];
326 if (start && si_->isValid(start) && si_->satisfiesBounds(start))
328 for (
unsigned int j = 0 ; j < states.size() && !path ; ++j)
329 if (si_->checkMotion(start, states[j]))
351 for (
unsigned int i = 0 ; i < startStates_.size() ; ++i)
353 const State *start = startStates_[i];
354 if (start && si_->isValid(start) && si_->satisfiesBounds(start))
357 if (goal_->isSatisfied(start, &dist))
377 return solutions_->getSolutionCount() > 0;
382 return solutions_->getSolutionCount();
387 return solutions_->getTopSolution();
401 solutions_->add(sol);
406 return solutions_->isApproximate();
411 return solutions_->isOptimized();
416 return solutions_->getDifference();
421 return solutions_->getSolutions();
431 out <<
"Start states:" << std::endl;
432 for (
unsigned int i = 0 ; i < startStates_.size() ; ++i)
433 si_->printState(startStates_[i], out);
437 out <<
"Goal = NULL" << std::endl;
438 if (optimizationObjective_)
439 out <<
"Average state cost: " << optimizationObjective_->averageStateCost(
magic::TEST_STATE_COUNT) << std::endl;
440 out <<
"There are " << solutions_->getSolutionCount() <<
" solutions" << std::endl;
445 return nonExistenceProof_.get();
450 nonExistenceProof_.reset();
455 return nonExistenceProof_;
460 nonExistenceProof_ = nonExistenceProof;
PathPtr getSolutionPath(void) const
Return the top solution path, if one is found. The top path is the shortest one that was found...
bool hasStartState(const State *state, unsigned int *startIndex=NULL)
Check whether a specified starting state is already included in the problem definition and optionally...
Representation of a solution to a planning problem.
void append(const base::State *state)
Append state to the end of the path; it is assumed state is the first state, so no control is applied...
Definition of an abstract control.
const SolutionNonExistenceProofPtr & getSolutionNonExistenceProof(void) const
Retrieve a pointer to the SolutionNonExistenceProof instance for this problem definition.
A boost shared pointer wrapper for ompl::base::SolutionNonExistenceProof.
Definition of a goal state.
void print(std::ostream &out=std::cout) const
Print information about the start and goal states and the optimization objective. ...
void addSolutionPath(const PathPtr &path, bool approximate=false, double difference=-1.0) const
Add a solution path in a thread-safe manner. Multiple solutions can be set for a goal. If a solution does not reach the desired goal it is considered approximate. Optionally, the distance between the desired goal and the one actually achieved is set by difference.
virtual std::size_t getStateCount(void) const
Return the number of valid goal states.
void setGoalState(const State *goal, const double threshold=std::numeric_limits< double >::epsilon())
A simple form of setting the goal. This is called by setStartAndGoalStates(). A more general form is ...
std::vector< PlannerSolution > getSolutions(void) const
Get all the solution paths available for this goal.
void setStartAndGoalStates(const State *start, const State *goal, const double threshold=std::numeric_limits< double >::epsilon())
In the simplest case possible, we have a single starting state and a single goal state.
bool hasOptimizedSolution(void) const
Return true if the top found solution is optimized (satisfies the specified optimization objective) ...
Definition of a set of goal states.
Definition of a control path.
bool isTrivial(unsigned int *startIndex=NULL, double *distance=NULL) const
A problem is trivial if a given starting state already in the goal region, so we need no motion plann...
bool hasSolution(void) const
Returns true if a solution path has been found (could be approximate)
virtual const State * getState(unsigned int index) const
Return a pointer to the indexth state in the state list.
void clearSolutionPaths(void) const
Forget the solution paths (thread safe). Memory is freed.
bool approximate_
True if goal was not achieved, but an approximate solution was found.
double getSolutionDifference(void) const
Get the distance to the desired goal for the top solution. Return -1.0 if there are no solutions avai...
void setState(const State *st)
Set the goal state.
std::size_t getSolutionCount(void) const
Get the number of solutions already found.
bool hasApproximateSolution(void) const
Return true if the top found solution is approximate (does not actually reach the desired goal...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr &nonExistenceProof)
Set the instance of SolutionNonExistenceProof for this problem definition.
PathPtr isStraightLinePathValid(void) const
Check if a straight line path is valid. If it is, return an instance of a path that represents the st...
bool fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts)
Many times the start or goal state will barely touch an obstacle. In this case, we may want to automa...
Definition of an abstract state.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
Helper function for fixInvalidInputStates(). Attempts to fix an individual state. ...
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
const State * getState(void) const
Get the goal state.
void setThreshold(double threshold)
Set the distance to the goal that is allowed for a state to be considered in the goal region...
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
ProblemDefinition(const SpaceInformationPtr &si)
Create a problem definition given the SpaceInformation it is part of.
A boost shared pointer wrapper for ompl::base::Goal.
Definition of a geometric path.
void clearSolutionNonExistenceProof(void)
Removes any existing instance of SolutionNonExistenceProof.
void getInputStates(std::vector< const State * > &states) const
Get all the input states. This includes start states and states that are part of goal regions that ca...
bool hasSolutionNonExistenceProof(void) const
Returns true if the problem definition has a proof of non existence for a solution.
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.