Loading...
Searching...
No Matches
ProblemDefinition.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2010, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Ioan Sucan */
36
37#ifndef OMPL_BASE_PROBLEM_DEFINITION_
38#define OMPL_BASE_PROBLEM_DEFINITION_
39
40#include "ompl/base/State.h"
41#include "ompl/base/Goal.h"
42#include "ompl/base/Path.h"
43#include "ompl/base/Cost.h"
44#include "ompl/base/SpaceInformation.h"
45#include "ompl/base/SolutionNonExistenceProof.h"
46#include "ompl/util/Console.h"
47#include "ompl/util/ClassForward.h"
48#include "ompl/base/ScopedState.h"
49
50#include <vector>
51#include <cstdlib>
52#include <iostream>
53#include <limits>
54
55namespace ompl
56{
57 namespace base
58 {
60
61 OMPL_CLASS_FORWARD(ProblemDefinition);
62 OMPL_CLASS_FORWARD(OptimizationObjective);
64
67
70 {
74 : path_(path)
75 , length_(path ? path->length() : std::numeric_limits<double>::infinity())
76 {
77 }
78
80 bool operator==(const PlannerSolution &p) const
81 {
82 return path_ == p.path_;
83 }
84
86 bool operator<(const PlannerSolution &b) const;
87
89 void setApproximate(double difference)
90 {
91 approximate_ = true;
92 difference_ = difference;
93 }
94
97 void setOptimized(const OptimizationObjectivePtr &opt, Cost cost, bool meetsObjective)
98 {
99 opt_ = opt;
100 cost_ = cost;
101 optimized_ = meetsObjective;
102 }
103
105 void setPlannerName(const std::string &name)
106 {
107 plannerName_ = name;
108 }
109
112 int index_{-1};
113
116
118 double length_;
119
121 bool approximate_{false};
122
124 double difference_{0.};
125
127 bool optimized_{false};
128
131
134
136 std::string plannerName_;
137 };
138
139 class Planner;
140
145 std::function<void(const Planner *, const std::vector<const base::State *> &, const Cost)>;
146
147 OMPL_CLASS_FORWARD(OptimizationObjective);
148
152 class ProblemDefinition
153 {
154 public:
155 // non-copyable
156 ProblemDefinition(const ProblemDefinition &) = delete;
157 ProblemDefinition &operator=(const ProblemDefinition &) = delete;
158
160 ProblemDefinition(SpaceInformationPtr si);
161
169
170 virtual ~ProblemDefinition()
171 {
173 }
174
177 {
178 return si_;
179 }
180
182 void addStartState(const State *state)
183 {
184 startStates_.push_back(si_->cloneState(state));
185 }
186
188 void addStartState(const ScopedState<> &state)
189 {
190 startStates_.push_back(si_->cloneState(state.get()));
191 }
192
196 bool hasStartState(const State *state, unsigned int *startIndex = nullptr) const;
197
200 {
201 for (auto &startState : startStates_)
202 si_->freeState(startState);
203 startStates_.clear();
204 }
205
207 unsigned int getStartStateCount() const
208 {
209 return startStates_.size();
210 }
211
213 const State *getStartState(unsigned int index) const
214 {
215 return startStates_[index];
216 }
217
219 State *getStartState(unsigned int index)
220 {
221 return startStates_[index];
222 }
223
225 void setGoal(const GoalPtr &goal)
226 {
227 goal_ = goal;
228 }
229
232 {
233 goal_.reset();
234 }
235
237 const GoalPtr &getGoal() const
238 {
239 return goal_;
240 }
241
246 void getInputStates(std::vector<const State *> &states) const;
247
255 void setStartAndGoalStates(const State *start, const State *goal,
256 double threshold = std::numeric_limits<double>::epsilon());
257
260 void setGoalState(const State *goal, double threshold = std::numeric_limits<double>::epsilon());
261
263 void setStartAndGoalStates(const ScopedState<> &start, const ScopedState<> &goal,
264 const double threshold = std::numeric_limits<double>::epsilon())
265 {
266 setStartAndGoalStates(start.get(), goal.get(), threshold);
267 }
268
270 void setGoalState(const ScopedState<> &goal,
271 const double threshold = std::numeric_limits<double>::epsilon())
272 {
273 setGoalState(goal.get(), threshold);
274 }
275
278 {
279 return optimizationObjective_ != nullptr;
280 }
281
287
289 void setOptimizationObjective(const OptimizationObjectivePtr &optimizationObjective)
290 {
291 optimizationObjective_ = optimizationObjective;
292 }
293
300
306
312 bool isTrivial(unsigned int *startIndex = nullptr, double *distance = nullptr) const;
313
327
334 bool fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts);
335
337 bool hasSolution() const;
338
341 bool hasExactSolution() const
342 {
343 return this->hasSolution() && !this->hasApproximateSolution();
344 }
345
349 bool hasApproximateSolution() const;
350
353 double getSolutionDifference() const;
354
357 bool hasOptimizedSolution() const;
358
363 PathPtr getSolutionPath() const;
364
370 bool getSolution(PlannerSolution &solution) const;
371
377 void addSolutionPath(const PathPtr &path, bool approximate = false, double difference = -1.0,
378 const std::string &plannerName = "Unknown") const;
379
381 void addSolutionPath(const PlannerSolution &sol) const;
382
384 std::size_t getSolutionCount() const;
385
387 std::vector<PlannerSolution> getSolutions() const;
388
390 void clearSolutionPaths() const;
391
393 bool hasSolutionNonExistenceProof() const;
394
397
400
402 void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr &nonExistenceProof);
403
405 void print(std::ostream &out = std::cout) const;
406
407 protected:
409 bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts);
410
413
415 std::vector<State *> startStates_;
416
419
422
425
428
429 private:
431 OMPL_CLASS_FORWARD(PlannerSolutionSet);
433
435 PlannerSolutionSetPtr solutions_;
436 };
437 }
438}
439
440#endif
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
A shared pointer wrapper for ompl::base::Goal.
A shared pointer wrapper for ompl::base::OptimizationObjective.
Abstract definition of optimization objectives.
A shared pointer wrapper for ompl::base::Path.
Base class for a planner.
Definition Planner.h:216
A shared pointer wrapper for ompl::base::ProblemDefinition.
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
void setGoal(const GoalPtr &goal)
Set the goal.
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...
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 ...
void clearSolutionNonExistenceProof()
Removes any existing instance of SolutionNonExistenceProof.
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...
std::size_t getSolutionCount() const
Get the number of solutions already found.
void addStartState(const State *state)
Add a start state. The state is copied.
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...
unsigned int getStartStateCount() const
Returns the number of start states.
GoalPtr goal_
The goal representation.
bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
Helper function for fixInvalidInputStates(). Attempts to fix an individual 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 std::string &plannerName="Unknown") const
Add a solution path in a thread-safe manner. Multiple solutions can be set for a goal....
const GoalPtr & getGoal() const
Return the current goal.
void setStartAndGoalStates(const ScopedState<> &start, const ScopedState<> &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 hasExactSolution() const
Returns true if an exact solution path has been found. Specifically returns hasSolution && !...
void clearStartStates()
Clear all start states (memory is freed)
const ReportIntermediateSolutionFn & getIntermediateSolutionCallback() const
When this function returns a valid function pointer, that function should be called by planners that ...
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...
const State * getStartState(unsigned int index) const
Returns a specific start state.
void addStartState(const ScopedState<> &state)
Add a start state. The state is copied.
void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr &nonExistenceProof)
Set the instance of SolutionNonExistenceProof for this problem definition.
PathPtr isStraightLinePathValid() const
Check if a straight line path is valid. If it is, return an instance of a path that represents the st...
void setGoalState(const ScopedState<> &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 ...
bool hasApproximateSolution() const
Return true if the top found solution is approximate (does not actually reach the desired goal,...
bool hasSolutionNonExistenceProof() const
Returns true if the problem definition has a proof of non existence for a solution.
SolutionNonExistenceProofPtr nonExistenceProof_
A Representation of a proof of non-existence of a solution for this problem definition.
bool hasSolution() const
Returns true if a solution path has been found (could be approximate)
bool hasOptimizationObjective() const
Check if an optimization objective was defined for planning.
bool getSolution(PlannerSolution &solution) const
Return true if a top solution is found, with the top solution passed by reference in the function hea...
State * getStartState(unsigned int index)
Returns a specific start state.
PathPtr getSolutionPath() const
Return the top solution path, if one is found. The top path is a shortest path that was found,...
const SolutionNonExistenceProofPtr & getSolutionNonExistenceProof() const
Retrieve a pointer to the SolutionNonExistenceProof instance for this problem definition.
void setOptimizationObjective(const OptimizationObjectivePtr &optimizationObjective)
Set the optimization objective to be considered during planning.
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.
OptimizationObjectivePtr optimizationObjective_
The objective to be optimized while solving the planning problem.
std::vector< PlannerSolution > getSolutions() const
Get all the solution paths available for this goal.
const OptimizationObjectivePtr & getOptimizationObjective() const
Get the optimization objective to be considered during planning.
ReportIntermediateSolutionFn intermediateSolutionCallback_
Callback function which is called when a new intermediate solution has been found.
SpaceInformationPtr si_
The space information this problem definition is for.
bool hasOptimizedSolution() const
Return true if the top found solution is optimized (satisfies the specified optimization objective)
void setIntermediateSolutionCallback(const ReportIntermediateSolutionFn &callback)
Set the callback to be called by planners that can compute intermediate solutions.
void clearSolutionPaths() const
Forget the solution paths (thread safe). Memory is freed.
ProblemDefinitionPtr clone() const
Return a copy of the problem definition.
void clearGoal()
Clear the goal. Memory is freed.
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this problem definition is for.
double getSolutionDifference() const
Get the distance to the desired goal for the top solution. Return -1.0 if there are no solutions avai...
std::vector< State * > startStates_
The set of start states.
Definition of a scoped state.
Definition ScopedState.h:57
StateType * get()
Returns a pointer to the contained state.
A shared pointer wrapper for ompl::base::SolutionNonExistenceProof.
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition State.h:50
This namespace contains sampling based planning routines shared by both planning under geometric cons...
std::function< void(const Planner *, const std::vector< const base::State * > &, const Cost)> ReportIntermediateSolutionFn
When a planner has an intermediate solution (e.g., optimizing planners), a function with this signatu...
Main namespace. Contains everything in this library.
STL namespace.
Representation of a solution to a planning problem.
PlannerSolution(const PathPtr &path)
Construct a solution that consists of a path and its attributes (whether it is approximate and the di...
int index_
When multiple solutions are found, each is given a number starting at 0, so that the order in which t...
bool optimized_
True if the solution was optimized to meet the specified optimization criterion.
std::string plannerName_
Name of planner type that generated this solution, as received from Planner::getName()
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
bool operator==(const PlannerSolution &p) const
Return true if two solutions are the same.
PathPtr path_
Solution path.
Cost cost_
The cost of this solution path, with respect to the optimization objective.
OptimizationObjectivePtr opt_
Optimization objective that was used to optimize this solution.
bool approximate_
True if goal was not achieved, but an approximate solution was found.
double length_
For efficiency reasons, keep the length of the path as well.
double difference_
The achieved difference between the found solution and the desired goal.
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
bool operator<(const PlannerSolution &b) const
Define a ranking for solutions.
void setOptimized(const OptimizationObjectivePtr &opt, Cost cost, bool meetsObjective)
Set the optimization objective used to optimize this solution, the cost of the solution and whether i...