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 
55 namespace ompl
56 {
57  namespace base
58  {
60 
61  OMPL_CLASS_FORWARD(ProblemDefinition);
62  OMPL_CLASS_FORWARD(OptimizationObjective);
64 
70  {
73  PlannerSolution(const PathPtr &path)
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 
144  typedef std::function<void(const Planner *, const std::vector<const base::State *> &, const Cost)>
146 
147  OMPL_CLASS_FORWARD(OptimizationObjective);
148 
153  {
154  public:
155  // non-copyable
156  ProblemDefinition(const ProblemDefinition &) = delete;
157  ProblemDefinition &operator=(const ProblemDefinition &) = delete;
158 
161 
162  virtual ~ProblemDefinition()
163  {
165  }
166 
169  {
170  return si_;
171  }
172 
174  void addStartState(const State *state)
175  {
176  startStates_.push_back(si_->cloneState(state));
177  }
178 
180  void addStartState(const ScopedState<> &state)
181  {
182  startStates_.push_back(si_->cloneState(state.get()));
183  }
184 
188  bool hasStartState(const State *state, unsigned int *startIndex = nullptr) const;
189 
192  {
193  for (auto &startState : startStates_)
194  si_->freeState(startState);
195  startStates_.clear();
196  }
197 
199  unsigned int getStartStateCount() const
200  {
201  return startStates_.size();
202  }
203 
205  const State *getStartState(unsigned int index) const
206  {
207  return startStates_[index];
208  }
209 
211  State *getStartState(unsigned int index)
212  {
213  return startStates_[index];
214  }
215 
217  void setGoal(const GoalPtr &goal)
218  {
219  goal_ = goal;
220  }
221 
223  void clearGoal()
224  {
225  goal_.reset();
226  }
227 
229  const GoalPtr &getGoal() const
230  {
231  return goal_;
232  }
233 
238  void getInputStates(std::vector<const State *> &states) const;
239 
247  void setStartAndGoalStates(const State *start, const State *goal,
248  double threshold = std::numeric_limits<double>::epsilon());
249 
252  void setGoalState(const State *goal, double threshold = std::numeric_limits<double>::epsilon());
253 
255  void setStartAndGoalStates(const ScopedState<> &start, const ScopedState<> &goal,
256  const double threshold = std::numeric_limits<double>::epsilon())
257  {
258  setStartAndGoalStates(start.get(), goal.get(), threshold);
259  }
260 
262  void setGoalState(const ScopedState<> &goal,
263  const double threshold = std::numeric_limits<double>::epsilon())
264  {
265  setGoalState(goal.get(), threshold);
266  }
267 
270  {
271  return optimizationObjective_ != nullptr;
272  }
273 
276  {
277  return optimizationObjective_;
278  }
279 
281  void setOptimizationObjective(const OptimizationObjectivePtr &optimizationObjective)
282  {
283  optimizationObjective_ = optimizationObjective;
284  }
285 
289  {
291  }
292 
295  {
297  }
298 
304  bool isTrivial(unsigned int *startIndex = nullptr, double *distance = nullptr) const;
305 
319 
326  bool fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts);
327 
329  bool hasSolution() const;
330 
333  bool hasExactSolution() const
334  {
335  return this->hasSolution() && !this->hasApproximateSolution();
336  }
337 
341  bool hasApproximateSolution() const;
342 
345  double getSolutionDifference() const;
346 
349  bool hasOptimizedSolution() const;
350 
355  PathPtr getSolutionPath() const;
356 
362  bool getSolution(PlannerSolution &solution) const;
363 
369  void addSolutionPath(const PathPtr &path, bool approximate = false, double difference = -1.0,
370  const std::string &plannerName = "Unknown") const;
371 
373  void addSolutionPath(const PlannerSolution &sol) const;
374 
376  std::size_t getSolutionCount() const;
377 
379  std::vector<PlannerSolution> getSolutions() const;
380 
382  void clearSolutionPaths() const;
383 
385  bool hasSolutionNonExistenceProof() const;
386 
389 
392 
394  void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr &nonExistenceProof);
395 
397  void print(std::ostream &out = std::cout) const;
398 
399  protected:
401  bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts);
402 
405 
407  std::vector<State *> startStates_;
408 
411 
414 
417 
420 
421  private:
423  OMPL_CLASS_FORWARD(PlannerSolutionSet);
425 
427  PlannerSolutionSetPtr solutions_;
428  };
429  }
430 }
431 
432 #endif
GoalPtr goal_
The goal representation.
const SolutionNonExistenceProofPtr & getSolutionNonExistenceProof() const
Retrieve a pointer to the SolutionNonExistenceProof instance for this problem definition.
void setGoal(const GoalPtr &goal)
Set the goal.
PlannerSolution(const PathPtr &path)
Construct a solution that consists of a path and its attributes (whether it is approximate and the di...
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
void print(std::ostream &out=std::cout) const
Print information about the start and goal states and the optimization objective. ...
bool hasApproximateSolution() const
Return true if the top found solution is approximate (does not actually reach the desired goal...
bool optimized_
True if the solution was optimized to meet the specified optimization criterion.
void setOptimizationObjective(const OptimizationObjectivePtr &optimizationObjective)
Set the optimization objective to be considered during planning.
Representation of a solution to a planning problem.
void clearSolutionNonExistenceProof()
Removes any existing instance of SolutionNonExistenceProof.
Definition of a scoped state.
Definition: ScopedState.h:56
State * getStartState(unsigned int index)
Returns a specific start state.
A shared pointer wrapper for ompl::base::SolutionNonExistenceProof.
ReportIntermediateSolutionFn intermediateSolutionCallback_
Callback function which is called when a new intermediate solution has been found.
PathPtr path_
Solution path.
bool hasOptimizedSolution() const
Return true if the top found solution is optimized (satisfies the specified optimization objective) ...
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...
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.
STL namespace.
bool hasSolutionNonExistenceProof() const
Returns true if the problem definition has a proof of non existence for a solution.
void addStartState(const ScopedState<> &state)
Add a start state. The state is copied.
SolutionNonExistenceProofPtr nonExistenceProof_
A Representation of a proof of non-existence of a solution for this problem definition.
StateType * get()
Returns a pointer to the contained state.
Definition: ScopedState.h:394
bool approximate_
True if goal was not achieved, but an approximate solution was found.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
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 && !hasApprox...
bool operator==(const PlannerSolution &p) const
Return true if two solutions are the same.
Base class for a planner.
Definition: Planner.h:223
PathPtr getSolutionPath() const
Return the top solution path, if one is found. The top path is the shortest one that was found...
std::vector< PlannerSolution > getSolutions() const
Get all the solution paths available for this goal.
void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr &nonExistenceProof)
Set the instance of SolutionNonExistenceProof for this problem definition.
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 ...
OptimizationObjectivePtr optimizationObjective_
The objective to be optimized while solving the planning problem.
const GoalPtr & getGoal() const
Return the current goal.
const State * getStartState(unsigned int index) const
Returns a specific start state.
int index_
When multiple solutions are found, each is given a number starting at 0, so that the order in which t...
PathPtr isStraightLinePathValid() 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...
void clearSolutionPaths() const
Forget the solution paths (thread safe). Memory is freed.
A shared pointer wrapper for ompl::base::SpaceInformation.
Cost cost_
The cost of this solution path, with respect to the optimization objective.
Definition of an abstract state.
Definition: State.h:49
void clearGoal()
Clear the goal. Memory is freed.
Abstract definition of optimization objectives.
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...
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.
bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
Helper function for fixInvalidInputStates(). Attempts to fix an individual state. ...
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
A shared pointer wrapper for ompl::base::OptimizationObjective.
OptimizationObjectivePtr opt_
Optimization objective that was used to optimize this solution.
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...
std::size_t getSolutionCount() const
Get the number of solutions already found.
std::string plannerName_
Name of planner type that generated this solution, as received from Planner::getName() ...
bool operator<(const PlannerSolution &b) const
Define a ranking for solutions.
SpaceInformationPtr si_
The space information this problem definition is for.
bool getSolution(PlannerSolution &solution) const
Return true if a top solution is found, with the top solution passed by reference in the function hea...
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 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...
void addStartState(const State *state)
Add a start state. The state is copied.
A shared pointer wrapper for ompl::base::Goal.
bool hasSolution() const
Returns true if a solution path has been found (could be approximate)
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...
double difference_
The achieved difference between the found solution and the desired goal.
bool hasOptimizationObjective() const
Check if an optimization objective was defined for planning.
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 ...
unsigned int getStartStateCount() const
Returns the number of start states.
double length_
For efficiency reasons, keep the length of the path as well.
const OptimizationObjectivePtr & getOptimizationObjective() const
Get the optimization objective to be considered during planning.
void setIntermediateSolutionCallback(const ReportIntermediateSolutionFn &callback)
Set the callback to be called by planners that can compute intermediate solutions.
const ReportIntermediateSolutionFn & getIntermediateSolutionCallback() const
When this function returns a valid function pointer, that function should be called by planners that ...
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
A shared pointer wrapper for ompl::base::Path.
std::vector< State * > startStates_
The set of start states.
void clearStartStates()
Clear all start states (memory is freed)