All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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/SpaceInformation.h"
44 #include "ompl/base/SolutionNonExistenceProof.h"
45 #include "ompl/util/Console.h"
46 #include "ompl/util/ClassForward.h"
47 #include "ompl/base/ScopedState.h"
48 
49 #include <vector>
50 #include <cstdlib>
51 #include <iostream>
52 #include <limits>
53 
54 #include <boost/noncopyable.hpp>
55 
56 namespace ompl
57 {
58  namespace base
59  {
60 
62 
63  OMPL_CLASS_FORWARD(ProblemDefinition);
65 
71  {
73  PlannerSolution(const PathPtr &path, bool approximate = false, double difference = -1.0) :
74  index_(-1), path_(path), length_(path->length()),
75  approximate_(approximate), difference_(difference),
76  optimized_(false)
77  {
78  }
79 
81  bool operator==(const PlannerSolution& p) const
82  {
83  return path_ == p.path_;
84  }
85 
87  bool operator<(const PlannerSolution &b) const
88  {
89  if (!approximate_ && b.approximate_)
90  return true;
91  if (approximate_ && !b.approximate_)
92  return false;
93  if (approximate_ && b.approximate_)
94  return difference_ < b.difference_;
95  if (optimized_ && !b.optimized_)
96  return true;
97  if (!optimized_ && b.optimized_)
98  return false;
99  return length_ < b.length_;
100  }
101 
103  int index_;
104 
107 
109  double length_;
110 
113 
115  double difference_;
116 
119  };
120 
121  OMPL_CLASS_FORWARD(OptimizationObjective);
122 
126  class ProblemDefinition : private boost::noncopyable
127  {
128  public:
129 
132 
133  virtual ~ProblemDefinition(void)
134  {
136  }
137 
140  {
141  return si_;
142  }
143 
145  void addStartState(const State *state)
146  {
147  startStates_.push_back(si_->cloneState(state));
148  }
149 
151  void addStartState(const ScopedState<> &state)
152  {
153  startStates_.push_back(si_->cloneState(state.get()));
154  }
155 
159  bool hasStartState(const State *state, unsigned int *startIndex = NULL);
160 
162  void clearStartStates(void)
163  {
164  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
165  si_->freeState(startStates_[i]);
166  startStates_.clear();
167  }
168 
170  unsigned int getStartStateCount(void) const
171  {
172  return startStates_.size();
173  }
174 
176  const State* getStartState(unsigned int index) const
177  {
178  return startStates_[index];
179  }
180 
182  State* getStartState(unsigned int index)
183  {
184  return startStates_[index];
185  }
186 
188  void setGoal(const GoalPtr &goal)
189  {
190  goal_ = goal;
191  }
192 
194  void clearGoal(void)
195  {
196  goal_.reset();
197  }
198 
200  const GoalPtr& getGoal(void) const
201  {
202  return goal_;
203  }
204 
209  void getInputStates(std::vector<const State*> &states) const;
210 
218  void setStartAndGoalStates(const State *start, const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
219 
221  void setGoalState(const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
222 
224  void setStartAndGoalStates(const ScopedState<> &start, const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
225  {
226  setStartAndGoalStates(start.get(), goal.get(), threshold);
227  }
228 
230  void setGoalState(const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
231  {
232  setGoalState(goal.get(), threshold);
233  }
234 
236  bool hasOptimizationObjective(void) const
237  {
238  return optimizationObjective_.get();
239  }
240 
243  {
244  return optimizationObjective_;
245  }
246 
248  void setOptimizationObjective(const OptimizationObjectivePtr &optimizationObjective)
249  {
250  optimizationObjective_ = optimizationObjective;
251  }
252 
258  bool isTrivial(unsigned int *startIndex = NULL, double *distance = NULL) const;
259 
272  PathPtr isStraightLinePathValid(void) const;
273 
278  bool fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts);
279 
281  bool hasSolution(void) const;
282 
286  bool hasApproximateSolution(void) const;
287 
289  double getSolutionDifference(void) const;
290 
292  bool hasOptimizedSolution(void) const;
293 
298  PathPtr getSolutionPath(void) const;
299 
304  void addSolutionPath(const PathPtr &path, bool approximate = false, double difference = -1.0) const;
305 
307  void addSolutionPath(const PlannerSolution &sol) const;
308 
310  std::size_t getSolutionCount(void) const;
311 
313  std::vector<PlannerSolution> getSolutions(void) const;
314 
316  void clearSolutionPaths(void) const;
317 
319  bool hasSolutionNonExistenceProof(void) const;
320 
323 
326 
328  void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr& nonExistenceProof);
329 
331  void print(std::ostream &out = std::cout) const;
332 
333  protected:
334 
336  bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts);
337 
340 
342  std::vector<State*> startStates_;
343 
346 
349 
352 
353  private:
354 
356  OMPL_CLASS_FORWARD(PlannerSolutionSet);
358 
360  PlannerSolutionSetPtr solutions_;
361  };
362  }
363 }
364 
365 #endif
GoalPtr goal_
The goal representation.
void setGoal(const GoalPtr &goal)
Set the goal.
PathPtr getSolutionPath(void) const
Return the top solution path, if one is found. The top path is the shortest one that was found...
bool optimized_
True of the solution was optimized to meet the specified optimization criterion.
void setOptimizationObjective(const OptimizationObjectivePtr &optimizationObjective)
Set the optimization objective to be considered during planning.
bool hasStartState(const State *state, unsigned int *startIndex=NULL)
Check whether a specified starting state is already included in the problem definition and optionally...
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.
const SolutionNonExistenceProofPtr & getSolutionNonExistenceProof(void) const
Retrieve a pointer to the SolutionNonExistenceProof instance for this problem definition.
const GoalPtr & getGoal(void) const
Return the current goal.
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. ...
PathPtr path_
Solution path.
unsigned int getStartStateCount(void) const
Returns the number of start states.
void addSolutionPath(const PathPtr &path, bool approximate=false, double difference=-1.0) 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.
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 ...
std::vector< PlannerSolution > getSolutions(void) const
Get all the solution paths available for this goal.
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.
bool hasOptimizedSolution(void) const
Return true if the top found solution is optimized (satisfies the specified optimization objective) ...
void clearStartStates(void)
Clear all start states (memory is freed)
void addStartState(const ScopedState<> &state)
Add a start state. The state is copied.
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...
bool hasSolution(void) const
Returns true if a solution path has been found (could be approximate)
void clearGoal(void)
Clear the goal. Memory is freed.
PlannerSolution(const PathPtr &path, bool approximate=false, double difference=-1.0)
Construct a solution that consists of a path and its attributes (whether it is approximate and the di...
SolutionNonExistenceProofPtr nonExistenceProof_
A Representation of a proof of non-existence of a solution for this problem definition.
const State * getStartState(unsigned int index) const
Returns a specific start state.
void clearSolutionPaths(void) const
Forget the solution paths (thread safe). Memory is freed.
bool approximate_
True if goal was not achieved, but an approximate solution was found.
double getSolutionDifference(void) const
Get the distance to the desired goal for the top solution. Return -1.0 if there are no solutions avai...
const SpaceInformationPtr & getSpaceInformation(void) const
Get the space information this problem definition is for.
bool operator<(const PlannerSolution &b) const
Define a ranking for solutions.
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.
std::size_t getSolutionCount(void) const
Get the number of solutions already found.
bool hasApproximateSolution(void) const
Return true if the top found solution is approximate (does not actually reach the desired goal...
void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr &nonExistenceProof)
Set the instance of SolutionNonExistenceProof for this problem definition.
PathPtr isStraightLinePathValid(void) 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 ...
OptimizationObjectivePtr optimizationObjective_
The objective to be optimized while solving the planning problem.
StateType * get(void)
Returns a pointer to the contained state.
Definition: ScopedState.h:396
int index_
When multiple solutions are found, each is given a number starting at 0, so that the order in which t...
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...
std::vector< State * > startStates_
The set of start states.
A boost shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
bool operator==(const PlannerSolution &p) const
Return true if two solutions are the same.
Abstract definition of optimization objectives.
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 boost shared pointer wrapper for ompl::base::OptimizationObjective.
const OptimizationObjectivePtr & getOptimizationObjective(void) const
Get the optimization objective to be considered during planning.
SpaceInformationPtr si_
The space information this problem definition is for.
ProblemDefinition(const SpaceInformationPtr &si)
Create a problem definition given the SpaceInformation it is part of.
void addStartState(const State *state)
Add a start state. The state is copied.
A boost shared pointer wrapper for ompl::base::Goal.
void clearSolutionNonExistenceProof(void)
Removes any existing instance of SolutionNonExistenceProof.
double difference_
The achieved difference between the found solution and the desired 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...
bool hasOptimizationObjective(void) const
Check if an optimization objective was defined for planning.
double length_
For efficiency reasons, keep the length of the path as well.
bool hasSolutionNonExistenceProof(void) const
Returns true if the problem definition has a proof of non existence for a solution.
A boost shared pointer wrapper for ompl::base::Path.