ProblemDefinition.cpp
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 #include "ompl/base/ProblemDefinition.h"
38 #include "ompl/base/goals/GoalState.h"
39 #include "ompl/base/goals/GoalStates.h"
40 #include "ompl/base/OptimizationObjective.h"
41 #include "ompl/control/SpaceInformation.h"
42 #include "ompl/control/PathControl.h"
43 #include "ompl/tools/config/MagicConstants.h"
44 #include <sstream>
45 #include <algorithm>
46 
47 #include <boost/thread/mutex.hpp>
48 
50 namespace ompl
51 {
52  namespace base
53  {
54 
55  class ProblemDefinition::PlannerSolutionSet
56  {
57  public:
58 
59  PlannerSolutionSet()
60  {
61  }
62 
63  void add(const PlannerSolution &s)
64  {
65  boost::mutex::scoped_lock slock(lock_);
66  int index = solutions_.size();
67  solutions_.push_back(s);
68  solutions_.back().index_ = index;
69  std::sort(solutions_.begin(), solutions_.end());
70  }
71 
72  void clear()
73  {
74  boost::mutex::scoped_lock slock(lock_);
75  solutions_.clear();
76  }
77 
78  std::vector<PlannerSolution> getSolutions()
79  {
80  boost::mutex::scoped_lock slock(lock_);
81  std::vector<PlannerSolution> copy = solutions_;
82  return copy;
83  }
84 
85  bool isApproximate()
86  {
87  boost::mutex::scoped_lock slock(lock_);
88  bool result = false;
89  if (!solutions_.empty())
90  result = solutions_[0].approximate_;
91  return result;
92  }
93 
94  bool isOptimized()
95  {
96  boost::mutex::scoped_lock slock(lock_);
97  bool result = false;
98  if (!solutions_.empty())
99  result = solutions_[0].optimized_;
100  return result;
101  }
102 
103  double getDifference()
104  {
105  boost::mutex::scoped_lock slock(lock_);
106  double diff = -1.0;
107  if (!solutions_.empty())
108  diff = solutions_[0].difference_;
109  return diff;
110  }
111 
112  PathPtr getTopSolution()
113  {
114  boost::mutex::scoped_lock slock(lock_);
115  PathPtr copy;
116  if (!solutions_.empty())
117  copy = solutions_[0].path_;
118  return copy;
119  }
120 
121  bool getTopSolution(PlannerSolution& solution)
122  {
123  boost::mutex::scoped_lock slock(lock_);
124 
125  if (!solutions_.empty())
126  {
127  solution = solutions_[0];
128  return true;
129  }
130  else
131  {
132  return false;
133  }
134  }
135 
136  std::size_t getSolutionCount()
137  {
138  boost::mutex::scoped_lock slock(lock_);
139  std::size_t result = solutions_.size();
140  return result;
141  }
142 
143  private:
144 
145  std::vector<PlannerSolution> solutions_;
146  boost::mutex lock_;
147  };
148  }
149 }
151 
153 {
154  if (!approximate_ && b.approximate_)
155  return true;
156  if (approximate_ && !b.approximate_)
157  return false;
158  if (approximate_ && b.approximate_)
159  return difference_ < b.difference_;
160  if (optimized_ && !b.optimized_)
161  return true;
162  if (!optimized_ && b.optimized_)
163  return false;
164  return opt_ ? opt_->isCostBetterThan(cost_, b.cost_) : length_ < b.length_;
165 }
166 
167 ompl::base::ProblemDefinition::ProblemDefinition(const SpaceInformationPtr &si) : si_(si), solutions_(new PlannerSolutionSet())
168 {
169 }
170 
171 void ompl::base::ProblemDefinition::setStartAndGoalStates(const State *start, const State *goal, const double threshold)
172 {
173  clearStartStates();
174  addStartState(start);
175  setGoalState(goal, threshold);
176 }
177 
178 void ompl::base::ProblemDefinition::setGoalState(const State *goal, const double threshold)
179 {
180  clearGoal();
181  GoalState *gs = new GoalState(si_);
182  gs->setState(goal);
183  gs->setThreshold(threshold);
184  setGoal(GoalPtr(gs));
185 }
186 
187 bool ompl::base::ProblemDefinition::hasStartState(const State *state, unsigned int *startIndex)
188 {
189  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
190  if (si_->equalStates(state, startStates_[i]))
191  {
192  if (startIndex)
193  *startIndex = i;
194  return true;
195  }
196  return false;
197 }
198 
199 bool ompl::base::ProblemDefinition::fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
200 {
201  bool result = false;
202 
203  bool b = si_->satisfiesBounds(state);
204  bool v = false;
205  if (b)
206  {
207  v = si_->isValid(state);
208  if (!v)
209  OMPL_DEBUG("%s state is not valid", start ? "Start" : "Goal");
210  }
211  else
212  OMPL_DEBUG("%s state is not within space bounds", start ? "Start" : "Goal");
213 
214  if (!b || !v)
215  {
216  std::stringstream ss;
217  si_->printState(state, ss);
218  ss << " within distance " << dist;
219  OMPL_DEBUG("Attempting to fix %s state %s", start ? "start" : "goal", ss.str().c_str());
220 
221  State *temp = si_->allocState();
222  if (si_->searchValidNearby(temp, state, dist, attempts))
223  {
224  si_->copyState(state, temp);
225  result = true;
226  }
227  else
228  OMPL_WARN("Unable to fix %s state", start ? "start" : "goal");
229  si_->freeState(temp);
230  }
231 
232  return result;
233 }
234 
235 bool ompl::base::ProblemDefinition::fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts)
236 {
237  bool result = true;
238 
239  // fix start states
240  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
241  if (!fixInvalidInputState(startStates_[i], distStart, true, attempts))
242  result = false;
243 
244  // fix goal state
245  GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
246  if (goal)
247  {
248  if (!fixInvalidInputState(const_cast<State*>(goal->getState()), distGoal, false, attempts))
249  result = false;
250  }
251 
252  // fix goal state
253  GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
254  if (goals)
255  {
256  for (unsigned int i = 0; i < goals->getStateCount(); ++i)
257  if (!fixInvalidInputState(const_cast<State*>(goals->getState(i)), distGoal, false, attempts))
258  result = false;
259  }
260 
261  return result;
262 }
263 
264 void ompl::base::ProblemDefinition::getInputStates(std::vector<const State*> &states) const
265 {
266  states.clear();
267  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
268  states.push_back(startStates_[i]);
269 
270  GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
271  if (goal)
272  states.push_back(goal->getState());
273 
274  GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
275  if (goals)
276  for (unsigned int i = 0; i < goals->getStateCount(); ++i)
277  states.push_back (goals->getState(i));
278 }
279 
281 {
282  PathPtr path;
283  if (control::SpaceInformationPtr sic = boost::dynamic_pointer_cast<control::SpaceInformation, SpaceInformation>(si_))
284  {
285  unsigned int startIndex;
286  if (isTrivial(&startIndex, NULL))
287  {
289  pc->append(startStates_[startIndex]);
290  control::Control *null = sic->allocControl();
291  sic->nullControl(null);
292  pc->append(startStates_[startIndex], null, 0.0);
293  sic->freeControl(null);
294  path.reset(pc);
295  }
296  else
297  {
298  control::Control *nc = sic->allocControl();
299  State *result1 = sic->allocState();
300  State *result2 = sic->allocState();
301  sic->nullControl(nc);
302 
303  for (unsigned int k = 0 ; k < startStates_.size() && !path ; ++k)
304  {
305  const State *start = startStates_[k];
306  if (start && si_->isValid(start) && si_->satisfiesBounds(start))
307  {
308  sic->copyState(result1, start);
309  for (unsigned int i = 0 ; i < sic->getMaxControlDuration() && !path ; ++i)
310  if (sic->propagateWhileValid(result1, nc, 1, result2))
311  {
312  if (goal_->isSatisfied(result2))
313  {
315  pc->append(start);
316  pc->append(result2, nc, (i + 1) * sic->getPropagationStepSize());
317  path.reset(pc);
318  break;
319  }
320  std::swap(result1, result2);
321  }
322  }
323  }
324  sic->freeState(result1);
325  sic->freeState(result2);
326  sic->freeControl(nc);
327  }
328  }
329  else
330  {
331  std::vector<const State*> states;
332  GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
333  if (goal)
334  if (si_->isValid(goal->getState()) && si_->satisfiesBounds(goal->getState()))
335  states.push_back(goal->getState());
336  GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
337  if (goals)
338  for (unsigned int i = 0; i < goals->getStateCount(); ++i)
339  if (si_->isValid(goals->getState(i)) && si_->satisfiesBounds(goals->getState(i)))
340  states.push_back(goals->getState(i));
341 
342  if (states.empty())
343  {
344  unsigned int startIndex;
345  if (isTrivial(&startIndex))
346  {
347  geometric::PathGeometric *pg = new geometric::PathGeometric(si_, startStates_[startIndex], startStates_[startIndex]);
348  path.reset(pg);
349  }
350  }
351  else
352  {
353  for (unsigned int i = 0 ; i < startStates_.size() && !path ; ++i)
354  {
355  const State *start = startStates_[i];
356  if (start && si_->isValid(start) && si_->satisfiesBounds(start))
357  {
358  for (unsigned int j = 0 ; j < states.size() && !path ; ++j)
359  if (si_->checkMotion(start, states[j]))
360  {
361  geometric::PathGeometric *pg = new geometric::PathGeometric(si_, start, states[j]);
362  path.reset(pg);
363  break;
364  }
365  }
366  }
367  }
368  }
369 
370  return path;
371 }
372 
373 bool ompl::base::ProblemDefinition::isTrivial(unsigned int *startIndex, double *distance) const
374 {
375  if (!goal_)
376  {
377  OMPL_ERROR("Goal undefined");
378  return false;
379  }
380 
381  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
382  {
383  const State *start = startStates_[i];
384  if (start && si_->isValid(start) && si_->satisfiesBounds(start))
385  {
386  double dist;
387  if (goal_->isSatisfied(start, &dist))
388  {
389  if (startIndex)
390  *startIndex = i;
391  if (distance)
392  *distance = dist;
393  return true;
394  }
395  }
396  else
397  {
398  OMPL_ERROR("Initial state is in collision!");
399  }
400  }
401 
402  return false;
403 }
404 
406 {
407  return solutions_->getSolutionCount() > 0;
408 }
409 
411 {
412  return solutions_->getSolutionCount();
413 }
414 
416 {
417  return solutions_->getTopSolution();
418 }
419 
421 {
422  return solutions_->getTopSolution(solution);
423 }
424 
425 void ompl::base::ProblemDefinition::addSolutionPath(const PathPtr &path, bool approximate, double difference, const std::string& plannerName) const
426 {
427  PlannerSolution sol(path);
428  if (approximate)
429  sol.setApproximate(difference);
430  sol.setPlannerName(plannerName);
431  addSolutionPath(sol);
432 }
433 
435 {
436  if (sol.approximate_)
437  OMPL_INFORM("ProblemDefinition: Adding approximate solution from planner %s", sol.plannerName_.c_str());
438  solutions_->add(sol);
439 }
440 
442 {
443  return solutions_->isApproximate();
444 }
445 
447 {
448  return solutions_->isOptimized();
449 }
450 
452 {
453  return solutions_->getDifference();
454 }
455 
456 std::vector<ompl::base::PlannerSolution> ompl::base::ProblemDefinition::getSolutions() const
457 {
458  return solutions_->getSolutions();
459 }
460 
462 {
463  solutions_->clear();
464 }
465 
466 void ompl::base::ProblemDefinition::print(std::ostream &out) const
467 {
468  out << "Start states:" << std::endl;
469  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
470  si_->printState(startStates_[i], out);
471  if (goal_)
472  goal_->print(out);
473  else
474  out << "Goal = NULL" << std::endl;
475  if (optimizationObjective_)
476  out << "Average state cost: " << optimizationObjective_->averageStateCost(magic::TEST_STATE_COUNT) << std::endl;
477  out << "There are " << solutions_->getSolutionCount() << " solutions" << std::endl;
478 }
479 
481 {
482  return nonExistenceProof_.get();
483 }
484 
486 {
487  nonExistenceProof_.reset();
488 }
489 
491 {
492  return nonExistenceProof_;
493 }
494 
496 {
497  nonExistenceProof_ = nonExistenceProof;
498 }
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.
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.
void clearSolutionNonExistenceProof()
Removes any existing instance of SolutionNonExistenceProof.
void append(const base::State *state)
Append state to the end of the path; it is assumed state is the first state, so no control is applied...
virtual std::size_t getStateCount() const
Return the number of valid goal states.
Definition: GoalStates.cpp:113
Definition of an abstract control.
Definition: Control.h:48
A boost shared pointer wrapper for ompl::base::SolutionNonExistenceProof.
const State * getState() const
Get the goal state.
Definition: GoalState.cpp:79
Definition of a goal state.
Definition: GoalState.h:50
void print(std::ostream &out=std::cout) const
Print information about the start and goal states and the optimization objective. ...
double getSolutionDifference() const
Get the distance to the desired goal for the top solution. Return -1.0 if there are no solutions avai...
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 ...
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.
Definition of a set of goal states.
Definition: GoalStates.h:50
Definition of a control path.
Definition: PathControl.h:60
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...
virtual const State * getState(unsigned int index) const
Return a pointer to the indexth state in the state list.
Definition: GoalStates.cpp:105
PathPtr getSolutionPath() const
Return the top solution path, if one is found. The top path is the shortest one that was found...
bool hasOptimizedSolution() const
Return true if the top found solution is optimized (satisfies the specified optimization objective) ...
bool hasSolution() const
Returns true if a solution path has been found (could be approximate)
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
bool operator<(const PlannerSolution &b) const
Define a ranking for solutions.
void clearSolutionPaths() const
Forget the solution paths (thread safe). Memory is freed.
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.
void setState(const State *st)
Set the goal state.
Definition: GoalState.cpp:67
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
bool hasApproximateSolution() 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.
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...
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
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
PathPtr isStraightLinePathValid() const
Check if a straight line path is valid. If it is, return an instance of a path that represents the st...
std::vector< PlannerSolution > getSolutions() const
Get all the solution paths available for this goal.
A boost shared pointer wrapper for ompl::control::SpaceInformation.
bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
Helper function for fixInvalidInputStates(). Attempts to fix an individual state. ...
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
OptimizationObjectivePtr opt_
Optimization objective that was used to optimize this solution.
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 setThreshold(double threshold)
Set the distance to the goal that is allowed for a state to be considered in the goal region...
Definition: GoalRegion.h:81
std::string plannerName_
Name of planner type that generated this solution, as received from Planner::getName() ...
bool hasSolutionNonExistenceProof() const
Returns true if the problem definition has a proof of non existence for a solution.
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
ProblemDefinition(const SpaceInformationPtr &si)
Create a problem definition given the SpaceInformation it is part of.
A boost shared pointer wrapper for ompl::base::Goal.
std::size_t getSolutionCount() const
Get the number of solutions already found.
Definition of a geometric path.
Definition: PathGeometric.h:60
const SolutionNonExistenceProofPtr & getSolutionNonExistenceProof() const
Retrieve a pointer to the SolutionNonExistenceProof instance for this problem definition.
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...
double length_
For efficiency reasons, keep the length of the path as well.
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68