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 #include <boost/noncopyable.hpp>
56 
57 namespace ompl
58 {
59  namespace base
60  {
61 
63 
64  OMPL_CLASS_FORWARD(ProblemDefinition);
65  OMPL_CLASS_FORWARD(OptimizationObjective);
67 
73  {
75  PlannerSolution(const PathPtr &path) :
76  index_(-1), path_(path), length_(path->length()),
77  approximate_(false), difference_(-1), optimized_(false)
78  {
79  }
80 
82  bool operator==(const PlannerSolution &p) const
83  {
84  return path_ == p.path_;
85  }
86 
88  bool operator<(const PlannerSolution &b) const;
89 
91  void setApproximate(double difference)
92  {
93  approximate_ = true;
94  difference_ = difference;
95  }
96 
98  void setOptimized(const OptimizationObjectivePtr &opt, Cost cost, bool meetsObjective)
99  {
100  opt_ = opt;
101  cost_ = cost;
102  optimized_ = meetsObjective;
103  }
104 
106  void setPlannerName(const std::string &name)
107  {
108  plannerName_ = name;
109  }
110 
112  int index_;
113 
116 
118  double length_;
119 
122 
124  double difference_;
125 
128 
131 
134 
136  std::string plannerName_;
137  };
138 
139  class Planner;
140 
143  typedef boost::function<void(const Planner*, const std::vector<const base::State*> &, const Cost)> ReportIntermediateSolutionFn;
144 
145  OMPL_CLASS_FORWARD(OptimizationObjective);
146 
150  class ProblemDefinition : private boost::noncopyable
151  {
152  public:
153 
156 
157  virtual ~ProblemDefinition()
158  {
159  clearStartStates();
160  }
161 
164  {
165  return si_;
166  }
167 
169  void addStartState(const State *state)
170  {
171  startStates_.push_back(si_->cloneState(state));
172  }
173 
175  void addStartState(const ScopedState<> &state)
176  {
177  startStates_.push_back(si_->cloneState(state.get()));
178  }
179 
183  bool hasStartState(const State *state, unsigned int *startIndex = NULL);
184 
187  {
188  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
189  si_->freeState(startStates_[i]);
190  startStates_.clear();
191  }
192 
194  unsigned int getStartStateCount() const
195  {
196  return startStates_.size();
197  }
198 
200  const State* getStartState(unsigned int index) const
201  {
202  return startStates_[index];
203  }
204 
206  State* getStartState(unsigned int index)
207  {
208  return startStates_[index];
209  }
210 
212  void setGoal(const GoalPtr &goal)
213  {
214  goal_ = goal;
215  }
216 
218  void clearGoal()
219  {
220  goal_.reset();
221  }
222 
224  const GoalPtr& getGoal() const
225  {
226  return goal_;
227  }
228 
233  void getInputStates(std::vector<const State*> &states) const;
234 
242  void setStartAndGoalStates(const State *start, const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
243 
245  void setGoalState(const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
246 
248  void setStartAndGoalStates(const ScopedState<> &start, const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
249  {
250  setStartAndGoalStates(start.get(), goal.get(), threshold);
251  }
252 
254  void setGoalState(const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
255  {
256  setGoalState(goal.get(), threshold);
257  }
258 
261  {
262  return optimizationObjective_.get();
263  }
264 
267  {
268  return optimizationObjective_;
269  }
270 
272  void setOptimizationObjective(const OptimizationObjectivePtr &optimizationObjective)
273  {
274  optimizationObjective_ = optimizationObjective;
275  }
276 
280  {
281  return intermediateSolutionCallback_;
282  }
283 
286  intermediateSolutionCallback_ = callback;
287  }
288 
294  bool isTrivial(unsigned int *startIndex = NULL, double *distance = NULL) const;
295 
308  PathPtr isStraightLinePathValid() const;
309 
314  bool fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts);
315 
317  bool hasSolution() const;
318 
322  bool hasApproximateSolution() const;
323 
325  double getSolutionDifference() const;
326 
328  bool hasOptimizedSolution() const;
329 
334  PathPtr getSolutionPath() const;
335 
339  bool getSolution(PlannerSolution& solution) const;
340 
346  void addSolutionPath(const PathPtr &path, bool approximate = false, double difference = -1.0, const std::string& plannerName = "Unknown") const;
347 
349  void addSolutionPath(const PlannerSolution &sol) const;
350 
352  std::size_t getSolutionCount() const;
353 
355  std::vector<PlannerSolution> getSolutions() const;
356 
358  void clearSolutionPaths() const;
359 
361  bool hasSolutionNonExistenceProof() const;
362 
364  void clearSolutionNonExistenceProof();
365 
367  const SolutionNonExistenceProofPtr& getSolutionNonExistenceProof() const;
368 
370  void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr& nonExistenceProof);
371 
373  void print(std::ostream &out = std::cout) const;
374 
375  protected:
376 
378  bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts);
379 
382 
384  std::vector<State*> startStates_;
385 
388 
391 
394 
397 
398  private:
399 
401  OMPL_CLASS_FORWARD(PlannerSolutionSet);
403 
405  PlannerSolutionSetPtr solutions_;
406  };
407  }
408 }
409 
410 #endif
GoalPtr goal_
The goal representation.
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.
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.
Definition of a scoped state.
Definition: ScopedState.h:56
State * getStartState(unsigned int index)
Returns a specific start state.
A boost 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.
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:396
bool approximate_
True if goal was not achieved, but an approximate solution was found.
Main namespace. Contains everything in this library.
Definition: Cost.h:42
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 operator==(const PlannerSolution &p) const
Return true if two solutions are the same.
Base class for a planner.
Definition: Planner.h:232
boost::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...
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...
std::vector< State * > startStates_
The set of start states.
A boost 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:50
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...
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
A boost shared pointer wrapper for ompl::base::OptimizationObjective.
OptimizationObjectivePtr opt_
Optimization objective that was used to optimize this solution.
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.
void addStartState(const State *state)
Add a start state. The state is copied.
A boost shared pointer wrapper for ompl::base::Goal.
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this problem definition is for.
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.
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 boost shared pointer wrapper for ompl::base::Path.
void clearStartStates()
Clear all start states (memory is freed)