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  : index_(-1)
75  , path_(path)
76  , length_(path ? path->length() : std::numeric_limits<double>::infinity())
77  , approximate_(false)
78  , difference_(-1)
79  , optimized_(false)
80  {
81  }
82 
84  bool operator==(const PlannerSolution &p) const
85  {
86  return path_ == p.path_;
87  }
88 
90  bool operator<(const PlannerSolution &b) const;
91 
93  void setApproximate(double difference)
94  {
95  approximate_ = true;
96  difference_ = difference;
97  }
98 
101  void setOptimized(const OptimizationObjectivePtr &opt, Cost cost, bool meetsObjective)
102  {
103  opt_ = opt;
104  cost_ = cost;
105  optimized_ = meetsObjective;
106  }
107 
109  void setPlannerName(const std::string &name)
110  {
111  plannerName_ = name;
112  }
113 
116  int index_;
117 
120 
122  double length_;
123 
126 
128  double difference_;
129 
132 
135 
138 
140  std::string plannerName_;
141  };
142 
143  class Planner;
144 
148  typedef std::function<void(const Planner *, const std::vector<const base::State *> &, const Cost)>
150 
151  OMPL_CLASS_FORWARD(OptimizationObjective);
152 
157  {
158  public:
159  // non-copyable
160  ProblemDefinition(const ProblemDefinition &) = delete;
161  ProblemDefinition &operator=(const ProblemDefinition &) = delete;
162 
165 
166  virtual ~ProblemDefinition()
167  {
168  clearStartStates();
169  }
170 
173  {
174  return si_;
175  }
176 
178  void addStartState(const State *state)
179  {
180  startStates_.push_back(si_->cloneState(state));
181  }
182 
184  void addStartState(const ScopedState<> &state)
185  {
186  startStates_.push_back(si_->cloneState(state.get()));
187  }
188 
192  bool hasStartState(const State *state, unsigned int *startIndex = nullptr) const;
193 
196  {
197  for (auto &startState : startStates_)
198  si_->freeState(startState);
199  startStates_.clear();
200  }
201 
203  unsigned int getStartStateCount() const
204  {
205  return startStates_.size();
206  }
207 
209  const State *getStartState(unsigned int index) const
210  {
211  return startStates_[index];
212  }
213 
215  State *getStartState(unsigned int index)
216  {
217  return startStates_[index];
218  }
219 
221  void setGoal(const GoalPtr &goal)
222  {
223  goal_ = goal;
224  }
225 
227  void clearGoal()
228  {
229  goal_.reset();
230  }
231 
233  const GoalPtr &getGoal() const
234  {
235  return goal_;
236  }
237 
242  void getInputStates(std::vector<const State *> &states) const;
243 
251  void setStartAndGoalStates(const State *start, const State *goal,
252  const double threshold = std::numeric_limits<double>::epsilon());
253 
256  void setGoalState(const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
257 
259  void setStartAndGoalStates(const ScopedState<> &start, const ScopedState<> &goal,
260  const double threshold = std::numeric_limits<double>::epsilon())
261  {
262  setStartAndGoalStates(start.get(), goal.get(), threshold);
263  }
264 
266  void setGoalState(const ScopedState<> &goal,
267  const double threshold = std::numeric_limits<double>::epsilon())
268  {
269  setGoalState(goal.get(), threshold);
270  }
271 
274  {
275  return optimizationObjective_.get();
276  }
277 
280  {
281  return optimizationObjective_;
282  }
283 
285  void setOptimizationObjective(const OptimizationObjectivePtr &optimizationObjective)
286  {
287  optimizationObjective_ = optimizationObjective;
288  }
289 
293  {
294  return intermediateSolutionCallback_;
295  }
296 
299  {
300  intermediateSolutionCallback_ = callback;
301  }
302 
308  bool isTrivial(unsigned int *startIndex = nullptr, double *distance = nullptr) const;
309 
322  PathPtr isStraightLinePathValid() const;
323 
330  bool fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts);
331 
333  bool hasSolution() const;
334 
337  bool hasExactSolution() const
338  {
339  return this->hasSolution() && !this->hasApproximateSolution();
340  }
341 
345  bool hasApproximateSolution() const;
346 
349  double getSolutionDifference() const;
350 
353  bool hasOptimizedSolution() const;
354 
359  PathPtr getSolutionPath() const;
360 
366  bool getSolution(PlannerSolution &solution) const;
367 
373  void addSolutionPath(const PathPtr &path, bool approximate = false, double difference = -1.0,
374  const std::string &plannerName = "Unknown") const;
375 
377  void addSolutionPath(const PlannerSolution &sol) const;
378 
380  std::size_t getSolutionCount() const;
381 
383  std::vector<PlannerSolution> getSolutions() const;
384 
386  void clearSolutionPaths() const;
387 
389  bool hasSolutionNonExistenceProof() const;
390 
392  void clearSolutionNonExistenceProof();
393 
395  const SolutionNonExistenceProofPtr &getSolutionNonExistenceProof() const;
396 
398  void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr &nonExistenceProof);
399 
401  void print(std::ostream &out = std::cout) const;
402 
403  protected:
405  bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts);
406 
409 
411  std::vector<State *> startStates_;
412 
415 
418 
421 
424 
425  private:
427  OMPL_CLASS_FORWARD(PlannerSolutionSet);
429 
431  PlannerSolutionSetPtr solutions_;
432  };
433  }
434 }
435 
436 #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 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.
STL namespace.
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:232
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...
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...
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::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 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 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)