ProblemDefinition.cpp
167 ompl::base::ProblemDefinition::ProblemDefinition(const SpaceInformationPtr &si) : si_(si), solutions_(new PlannerSolutionSet())
171 void ompl::base::ProblemDefinition::setStartAndGoalStates(const State *start, const State *goal, const double threshold)
199 bool ompl::base::ProblemDefinition::fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
235 bool ompl::base::ProblemDefinition::fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts)
283 if (control::SpaceInformationPtr sic = boost::dynamic_pointer_cast<control::SpaceInformation, SpaceInformation>(si_))
347 geometric::PathGeometric *pg = new geometric::PathGeometric(si_, startStates_[startIndex], startStates_[startIndex]);
425 void ompl::base::ProblemDefinition::addSolutionPath(const PathPtr &path, bool approximate, double difference, const std::string& plannerName) const
437 OMPL_INFORM("ProblemDefinition: Adding approximate solution from planner %s", sol.plannerName_.c_str());
476 out << "Average state cost: " << optimizationObjective_->averageStateCost(magic::TEST_STATE_COUNT) << std::endl;
490 const ompl::base::SolutionNonExistenceProofPtr& ompl::base::ProblemDefinition::getSolutionNonExistenceProof() const
495 void ompl::base::ProblemDefinition::setSolutionNonExistenceProof(const ompl::base::SolutionNonExistenceProofPtr& nonExistenceProof)
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
Definition: ProblemDefinition.h:91
bool optimized_
True if the solution was optimized to meet the specified optimization criterion.
Definition: ProblemDefinition.h:127
bool hasStartState(const State *state, unsigned int *startIndex=NULL)
Check whether a specified starting state is already included in the problem definition and optionally...
Definition: ProblemDefinition.cpp:187
Representation of a solution to a planning problem.
Definition: ProblemDefinition.h:72
void clearSolutionNonExistenceProof()
Removes any existing instance of SolutionNonExistenceProof.
Definition: ProblemDefinition.cpp:485
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: PathControl.cpp:266
virtual std::size_t getStateCount() const
Return the number of valid goal states.
Definition: GoalStates.cpp:113
A boost shared pointer wrapper for ompl::base::SolutionNonExistenceProof.
void print(std::ostream &out=std::cout) const
Print information about the start and goal states and the optimization objective. ...
Definition: ProblemDefinition.cpp:466
double getSolutionDifference() const
Get the distance to the desired goal for the top solution. Return -1.0 if there are no solutions avai...
Definition: ProblemDefinition.cpp:451
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 ...
Definition: ProblemDefinition.cpp:178
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.
Definition: ProblemDefinition.cpp:171
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...
Definition: ProblemDefinition.cpp:373
virtual const State * getState(unsigned int index) const
Return a pointer to the indexth state in the state list.
Definition: GoalStates.cpp:105
PathPtr getSolutionPath() const
Return the top solution path, if one is found. The top path is the shortest one that was found...
Definition: ProblemDefinition.cpp:415
SolutionNonExistenceProofPtr nonExistenceProof_
A Representation of a proof of non-existence of a solution for this problem definition.
Definition: ProblemDefinition.h:390
bool hasOptimizedSolution() const
Return true if the top found solution is optimized (satisfies the specified optimization objective) ...
Definition: ProblemDefinition.cpp:446
bool hasSolution() const
Returns true if a solution path has been found (could be approximate)
Definition: ProblemDefinition.cpp:405
bool approximate_
True if goal was not achieved, but an approximate solution was found.
Definition: ProblemDefinition.h:121
bool operator<(const PlannerSolution &b) const
Define a ranking for solutions.
Definition: ProblemDefinition.cpp:152
void clearSolutionPaths() const
Forget the solution paths (thread safe). Memory is freed.
Definition: ProblemDefinition.cpp:461
void addSolutionPath(const PathPtr &path, bool approximate=false, double difference=-1.0, const std::string &plannerName="Unknown") 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. Optionally, the name of the planner that generated the solution.
Definition: ProblemDefinition.cpp:425
bool hasApproximateSolution() const
Return true if the top found solution is approximate (does not actually reach the desired goal...
Definition: ProblemDefinition.cpp:441
void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr &nonExistenceProof)
Set the instance of SolutionNonExistenceProof for this problem definition.
Definition: ProblemDefinition.cpp:495
OptimizationObjectivePtr optimizationObjective_
The objective to be optimized while solving the planning problem.
Definition: ProblemDefinition.h:393
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: ProblemDefinition.cpp:235
A boost shared pointer wrapper for ompl::base::SpaceInformation.
Cost cost_
The cost of this solution path, with respect to the optimization objective.
Definition: ProblemDefinition.h:133
PathPtr isStraightLinePathValid() const
Check if a straight line path is valid. If it is, return an instance of a path that represents the st...
Definition: ProblemDefinition.cpp:280
std::vector< PlannerSolution > getSolutions() const
Get all the solution paths available for this goal.
Definition: ProblemDefinition.cpp:456
A boost shared pointer wrapper for ompl::control::SpaceInformation.
bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
Helper function for fixInvalidInputStates(). Attempts to fix an individual state. ...
Definition: ProblemDefinition.cpp:199
bool getSolution(PlannerSolution &solution) const
Return true if a top solution is found, with the top solution passed by reference in the function hea...
Definition: ProblemDefinition.cpp:420
void setThreshold(double threshold)
Set the distance to the goal that is allowed for a state to be considered in the goal region...
Definition: GoalRegion.h:81
std::string plannerName_
Name of planner type that generated this solution, as received from Planner::getName() ...
Definition: ProblemDefinition.h:136
SpaceInformationPtr si_
The space information this problem definition is for.
Definition: ProblemDefinition.h:381
bool hasSolutionNonExistenceProof() const
Returns true if the problem definition has a proof of non existence for a solution.
Definition: ProblemDefinition.cpp:480
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
Definition: MagicConstants.h:109
ProblemDefinition(const SpaceInformationPtr &si)
Create a problem definition given the SpaceInformation it is part of.
Definition: ProblemDefinition.cpp:167
void addStartState(const State *state)
Add a start state. The state is copied.
Definition: ProblemDefinition.h:169
A boost shared pointer wrapper for ompl::base::Goal.
std::size_t getSolutionCount() const
Get the number of solutions already found.
Definition: ProblemDefinition.cpp:410
const SolutionNonExistenceProofPtr & getSolutionNonExistenceProof() const
Retrieve a pointer to the SolutionNonExistenceProof instance for this problem definition.
Definition: ProblemDefinition.cpp:490
double difference_
The achieved difference between the found solution and the desired goal.
Definition: ProblemDefinition.h:124
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...
Definition: ProblemDefinition.cpp:264
double length_
For efficiency reasons, keep the length of the path as well.
Definition: ProblemDefinition.h:118
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
Definition: ProblemDefinition.h:106
A boost shared pointer wrapper for ompl::base::Path.