All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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(void)
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(void)
73  {
74  boost::mutex::scoped_lock slock(lock_);
75  solutions_.clear();
76  }
77 
78  std::vector<PlannerSolution> getSolutions(void)
79  {
80  boost::mutex::scoped_lock slock(lock_);
81  std::vector<PlannerSolution> copy = solutions_;
82  return copy;
83  }
84 
85  bool isApproximate(void)
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(void)
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(void)
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(void)
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  std::size_t getSolutionCount(void)
122  {
123  boost::mutex::scoped_lock slock(lock_);
124  std::size_t result = solutions_.size();
125  return result;
126  }
127 
128  private:
129 
130  std::vector<PlannerSolution> solutions_;
131  boost::mutex lock_;
132  };
133  }
134 }
136 
137 ompl::base::ProblemDefinition::ProblemDefinition(const SpaceInformationPtr &si) : si_(si), solutions_(new PlannerSolutionSet())
138 {
139 }
140 
141 void ompl::base::ProblemDefinition::setStartAndGoalStates(const State *start, const State *goal, const double threshold)
142 {
143  clearStartStates();
144  addStartState(start);
145  setGoalState(goal, threshold);
146 }
147 
148 void ompl::base::ProblemDefinition::setGoalState(const State *goal, const double threshold)
149 {
150  clearGoal();
151  GoalState *gs = new GoalState(si_);
152  gs->setState(goal);
153  gs->setThreshold(threshold);
154  setGoal(GoalPtr(gs));
155 }
156 
157 bool ompl::base::ProblemDefinition::hasStartState(const State *state, unsigned int *startIndex)
158 {
159  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
160  if (si_->equalStates(state, startStates_[i]))
161  {
162  if (startIndex)
163  *startIndex = i;
164  return true;
165  }
166  return false;
167 }
168 
169 bool ompl::base::ProblemDefinition::fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
170 {
171  bool result = false;
172 
173  bool b = si_->satisfiesBounds(state);
174  bool v = false;
175  if (b)
176  {
177  v = si_->isValid(state);
178  if (!v)
179  OMPL_DEBUG("%s state is not valid", start ? "Start" : "Goal");
180  }
181  else
182  OMPL_DEBUG("%s state is not within space bounds", start ? "Start" : "Goal");
183 
184  if (!b || !v)
185  {
186  std::stringstream ss;
187  si_->printState(state, ss);
188  ss << " within distance " << dist;
189  OMPL_DEBUG("Attempting to fix %s state %s", start ? "start" : "goal", ss.str().c_str());
190 
191  State *temp = si_->allocState();
192  if (si_->searchValidNearby(temp, state, dist, attempts))
193  {
194  si_->copyState(state, temp);
195  result = true;
196  }
197  else
198  OMPL_WARN("Unable to fix %s state", start ? "start" : "goal");
199  si_->freeState(temp);
200  }
201 
202  return result;
203 }
204 
205 bool ompl::base::ProblemDefinition::fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts)
206 {
207  bool result = true;
208 
209  // fix start states
210  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
211  if (!fixInvalidInputState(startStates_[i], distStart, true, attempts))
212  result = false;
213 
214  // fix goal state
215  GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
216  if (goal)
217  {
218  if (!fixInvalidInputState(const_cast<State*>(goal->getState()), distGoal, false, attempts))
219  result = false;
220  }
221 
222  // fix goal state
223  GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
224  if (goals)
225  {
226  for (unsigned int i = 0; i < goals->getStateCount(); ++i)
227  if (!fixInvalidInputState(const_cast<State*>(goals->getState(i)), distGoal, false, attempts))
228  result = false;
229  }
230 
231  return result;
232 }
233 
234 void ompl::base::ProblemDefinition::getInputStates(std::vector<const State*> &states) const
235 {
236  states.clear();
237  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
238  states.push_back(startStates_[i]);
239 
240  GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
241  if (goal)
242  states.push_back(goal->getState());
243 
244  GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
245  if (goals)
246  for (unsigned int i = 0; i < goals->getStateCount(); ++i)
247  states.push_back (goals->getState(i));
248 }
249 
251 {
252  PathPtr path;
253  if (control::SpaceInformationPtr sic = boost::dynamic_pointer_cast<control::SpaceInformation, SpaceInformation>(si_))
254  {
255  unsigned int startIndex;
256  if (isTrivial(&startIndex, NULL))
257  {
259  pc->append(startStates_[startIndex]);
260  control::Control *null = sic->allocControl();
261  sic->nullControl(null);
262  pc->append(startStates_[startIndex], null, 0.0);
263  sic->freeControl(null);
264  path.reset(pc);
265  }
266  else
267  {
268  control::Control *nc = sic->allocControl();
269  State *result1 = sic->allocState();
270  State *result2 = sic->allocState();
271  sic->nullControl(nc);
272 
273  for (unsigned int k = 0 ; k < startStates_.size() && !path ; ++k)
274  {
275  const State *start = startStates_[k];
276  if (start && si_->isValid(start) && si_->satisfiesBounds(start))
277  {
278  sic->copyState(result1, start);
279  for (unsigned int i = 0 ; i < sic->getMaxControlDuration() && !path ; ++i)
280  if (sic->propagateWhileValid(result1, nc, 1, result2))
281  {
282  if (goal_->isSatisfied(result2))
283  {
285  pc->append(start);
286  pc->append(result2, nc, (i + 1) * sic->getPropagationStepSize());
287  path.reset(pc);
288  break;
289  }
290  std::swap(result1, result2);
291  }
292  }
293  }
294  sic->freeState(result1);
295  sic->freeState(result2);
296  sic->freeControl(nc);
297  }
298  }
299  else
300  {
301  std::vector<const State*> states;
302  GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
303  if (goal)
304  if (si_->isValid(goal->getState()) && si_->satisfiesBounds(goal->getState()))
305  states.push_back(goal->getState());
306  GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
307  if (goals)
308  for (unsigned int i = 0; i < goals->getStateCount(); ++i)
309  if (si_->isValid(goals->getState(i)) && si_->satisfiesBounds(goals->getState(i)))
310  states.push_back(goals->getState(i));
311 
312  if (states.empty())
313  {
314  unsigned int startIndex;
315  if (isTrivial(&startIndex))
316  {
317  geometric::PathGeometric *pg = new geometric::PathGeometric(si_, startStates_[startIndex], startStates_[startIndex]);
318  path.reset(pg);
319  }
320  }
321  else
322  {
323  for (unsigned int i = 0 ; i < startStates_.size() && !path ; ++i)
324  {
325  const State *start = startStates_[i];
326  if (start && si_->isValid(start) && si_->satisfiesBounds(start))
327  {
328  for (unsigned int j = 0 ; j < states.size() && !path ; ++j)
329  if (si_->checkMotion(start, states[j]))
330  {
331  geometric::PathGeometric *pg = new geometric::PathGeometric(si_, start, states[j]);
332  path.reset(pg);
333  break;
334  }
335  }
336  }
337  }
338  }
339 
340  return path;
341 }
342 
343 bool ompl::base::ProblemDefinition::isTrivial(unsigned int *startIndex, double *distance) const
344 {
345  if (!goal_)
346  {
347  OMPL_ERROR("Goal undefined");
348  return false;
349  }
350 
351  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
352  {
353  const State *start = startStates_[i];
354  if (start && si_->isValid(start) && si_->satisfiesBounds(start))
355  {
356  double dist;
357  if (goal_->isSatisfied(start, &dist))
358  {
359  if (startIndex)
360  *startIndex = i;
361  if (distance)
362  *distance = dist;
363  return true;
364  }
365  }
366  else
367  {
368  OMPL_ERROR("Initial state is in collision!");
369  }
370  }
371 
372  return false;
373 }
374 
376 {
377  return solutions_->getSolutionCount() > 0;
378 }
379 
381 {
382  return solutions_->getSolutionCount();
383 }
384 
386 {
387  return solutions_->getTopSolution();
388 }
389 
390 void ompl::base::ProblemDefinition::addSolutionPath(const PathPtr &path, bool approximate, double difference) const
391 {
392  if (approximate)
393  OMPL_INFORM("Adding approximate solution");
394  solutions_->add(PlannerSolution(path, approximate, difference));
395 }
396 
398 {
399  if (sol.approximate_)
400  OMPL_INFORM("Adding approximate solution");
401  solutions_->add(sol);
402 }
403 
405 {
406  return solutions_->isApproximate();
407 }
408 
410 {
411  return solutions_->isOptimized();
412 }
413 
415 {
416  return solutions_->getDifference();
417 }
418 
419 std::vector<ompl::base::PlannerSolution> ompl::base::ProblemDefinition::getSolutions(void) const
420 {
421  return solutions_->getSolutions();
422 }
423 
425 {
426  solutions_->clear();
427 }
428 
429 void ompl::base::ProblemDefinition::print(std::ostream &out) const
430 {
431  out << "Start states:" << std::endl;
432  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
433  si_->printState(startStates_[i], out);
434  if (goal_)
435  goal_->print(out);
436  else
437  out << "Goal = NULL" << std::endl;
438  if (optimizationObjective_)
439  out << "Average state cost: " << optimizationObjective_->averageStateCost(magic::TEST_STATE_COUNT) << std::endl;
440  out << "There are " << solutions_->getSolutionCount() << " solutions" << std::endl;
441 }
442 
444 {
445  return nonExistenceProof_.get();
446 }
447 
449 {
450  nonExistenceProof_.reset();
451 }
452 
454 {
455  return nonExistenceProof_;
456 }
457 
459 {
460  nonExistenceProof_ = nonExistenceProof;
461 }
PathPtr getSolutionPath(void) const
Return the top solution path, if one is found. The top path is the shortest one that was found...
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 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...
Definition of an abstract control.
Definition: Control.h:48
const SolutionNonExistenceProofPtr & getSolutionNonExistenceProof(void) const
Retrieve a pointer to the SolutionNonExistenceProof instance for this problem definition.
A boost shared pointer wrapper for ompl::base::SolutionNonExistenceProof.
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. ...
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.
virtual std::size_t getStateCount(void) const
Return the number of valid goal states.
Definition: GoalStates.cpp:113
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) ...
Definition of a set of goal states.
Definition: GoalStates.h:50
Definition of a control path.
Definition: PathControl.h:54
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)
virtual const State * getState(unsigned int index) const
Return a pointer to the indexth state in the state list.
Definition: GoalStates.cpp:105
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...
void setState(const State *st)
Set the goal state.
Definition: GoalState.cpp:67
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...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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...
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.
Definition of an abstract state.
Definition: State.h:50
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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
const State * getState(void) const
Get the goal state.
Definition: GoalState.cpp:79
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
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.
Definition of a geometric path.
Definition: PathGeometric.h:55
void clearSolutionNonExistenceProof(void)
Removes any existing instance of SolutionNonExistenceProof.
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 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.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68