ProblemDefinition.cpp
181 void ompl::base::ProblemDefinition::setStartAndGoalStates(const State *start, const State *goal, const double threshold)
197 bool ompl::base::ProblemDefinition::hasStartState(const State *state, unsigned int *startIndex) const
209 bool ompl::base::ProblemDefinition::fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
245 bool ompl::base::ProblemDefinition::fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts)
293 if (control::SpaceInformationPtr sic = std::dynamic_pointer_cast<control::SpaceInformation, SpaceInformation>(si_))
357 std::make_shared<geometric::PathGeometric>(si_, startStates_[startIndex], startStates_[startIndex]);
424 void ompl::base::ProblemDefinition::addSolutionPath(const PathPtr &path, bool approximate, double difference,
437 OMPL_INFORM("ProblemDefinition: Adding approximate solution from planner %s", sol.plannerName_.c_str());
478 out << "Average state cost: " << optimizationObjective_->averageStateCost(magic::TEST_STATE_COUNT) << std::endl;
void clearSolutionNonExistenceProof()
Removes any existing instance of SolutionNonExistenceProof.
Definition: ProblemDefinition.cpp:490
A shared pointer wrapper for ompl::base::Path.
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:165
A shared pointer wrapper for ompl::base::SpaceInformation.
bool hasOptimizedSolution() const
Return true if the top found solution is optimized (satisfies the specified optimization objective)
Definition: ProblemDefinition.cpp:446
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
Definition: ProblemDefinition.h:153
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....
Definition: ProblemDefinition.cpp:424
OptimizationObjectivePtr opt_
Optimization objective that was used to optimize this solution.
Definition: ProblemDefinition.h:194
std::string plannerName_
Name of planner type that generated this solution, as received from Planner::getName()
Definition: ProblemDefinition.h:200
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:290
Representation of a solution to a planning problem.
Definition: ProblemDefinition.h:133
bool hasStartState(const State *state, unsigned int *startIndex=nullptr) const
Check whether a specified starting state is already included in the problem definition and optionally...
Definition: ProblemDefinition.cpp:197
std::vector< PlannerSolution > getSolutions() const
Get all the solution paths available for this goal.
Definition: ProblemDefinition.cpp:456
std::size_t getSolutionCount() const
Get the number of solutions already found.
Definition: ProblemDefinition.cpp:409
double length_
For efficiency reasons, keep the length of the path as well.
Definition: ProblemDefinition.h:182
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:245
bool approximate_
True if goal was not achieved, but an approximate solution was found.
Definition: ProblemDefinition.h:185
bool hasSolution() const
Returns true if a solution path has been found (could be approximate)
Definition: ProblemDefinition.cpp:404
bool operator<(const PlannerSolution &b) const
Define a ranking for solutions.
Definition: ProblemDefinition.cpp:148
void setGoalState(const State *goal, 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:188
A shared pointer wrapper for ompl::base::ProblemDefinition.
bool hasSolutionNonExistenceProof() const
Returns true if the problem definition has a proof of non existence for a solution.
Definition: ProblemDefinition.cpp:485
bool isTrivial(unsigned int *startIndex=nullptr, double *distance=nullptr) const
A problem is trivial if a given starting state already in the goal region, so we need no motion plann...
Definition: ProblemDefinition.cpp:372
Cost cost_
The cost of this solution path, with respect to the optimization objective.
Definition: ProblemDefinition.h:197
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 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:274
ProblemDefinitionPtr clone() const
Return a copy of the problem definition.
Definition: ProblemDefinition.cpp:168
bool optimized_
True if the solution was optimized to meet the specified optimization criterion.
Definition: ProblemDefinition.h:191
A shared pointer wrapper for ompl::base::SolutionNonExistenceProof.
bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
Helper function for fixInvalidInputStates(). Attempts to fix an individual state.
Definition: ProblemDefinition.cpp:209
PathPtr getSolutionPath() const
Return the top solution path, if one is found. The top path is a shortest path that was found,...
Definition: ProblemDefinition.cpp:414
double difference_
The achieved difference between the found solution and the desired goal.
Definition: ProblemDefinition.h:188
void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr &nonExistenceProof)
Set the instance of SolutionNonExistenceProof for this problem definition.
Definition: ProblemDefinition.cpp:500
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
Definition: ProblemDefinition.h:169
void clearSolutionPaths() const
Forget the solution paths (thread safe). Memory is freed.
Definition: ProblemDefinition.cpp:461
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:419
const SolutionNonExistenceProofPtr & getSolutionNonExistenceProof() const
Retrieve a pointer to the SolutionNonExistenceProof instance for this problem definition.
Definition: ProblemDefinition.cpp:495
void print(std::ostream &out=std::cout) const
Print information about the start and goal states and the optimization objective.
Definition: ProblemDefinition.cpp:466
bool hasApproximateSolution() const
Return true if the top found solution is approximate (does not actually reach the desired goal,...
Definition: ProblemDefinition.cpp:441
void setStartAndGoalStates(const State *start, const State *goal, 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:181