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/control/SpaceInformation.h"
41 #include "ompl/control/PathControl.h"
42 #include <sstream>
43 #include <algorithm>
44 
45 #include <boost/thread/mutex.hpp>
46 
48 namespace ompl
49 {
50  namespace base
51  {
52 
53  class ProblemDefinition::PlannerSolutionSet
54  {
55  public:
56 
57  PlannerSolutionSet(void)
58  {
59  }
60 
61  void add(const PlannerSolution &s)
62  {
63  boost::mutex::scoped_lock slock(lock_);
64  int index = solutions_.size();
65  solutions_.push_back(s);
66  solutions_.back().index_ = index;
67  std::sort(solutions_.begin(), solutions_.end());
68  }
69 
70  void clear(void)
71  {
72  boost::mutex::scoped_lock slock(lock_);
73  solutions_.clear();
74  }
75 
76  std::vector<PlannerSolution> getSolutions(void)
77  {
78  boost::mutex::scoped_lock slock(lock_);
79  std::vector<PlannerSolution> copy = solutions_;
80  return copy;
81  }
82 
83  bool isApproximate(void)
84  {
85  boost::mutex::scoped_lock slock(lock_);
86  bool result = false;
87  if (!solutions_.empty())
88  result = solutions_[0].approximate_;
89  return result;
90  }
91 
92  double getDifference(void)
93  {
94  boost::mutex::scoped_lock slock(lock_);
95  double diff = -1.0;
96  if (!solutions_.empty())
97  diff = solutions_[0].difference_;
98  return diff;
99  }
100 
101  PathPtr getTopSolution(void)
102  {
103  boost::mutex::scoped_lock slock(lock_);
104  PathPtr copy;
105  if (!solutions_.empty())
106  copy = solutions_[0].path_;
107  return copy;
108  }
109 
110  std::size_t getSolutionCount(void)
111  {
112  boost::mutex::scoped_lock slock(lock_);
113  std::size_t result = solutions_.size();
114  return result;
115  }
116 
117  private:
118 
119  std::vector<PlannerSolution> solutions_;
120  boost::mutex lock_;
121  };
122  }
123 }
125 
126 ompl::base::ProblemDefinition::ProblemDefinition(const SpaceInformationPtr &si) : si_(si), solutions_(new PlannerSolutionSet())
127 {
128 }
129 
130 void ompl::base::ProblemDefinition::setStartAndGoalStates(const State *start, const State *goal, const double threshold)
131 {
132  clearStartStates();
133  addStartState(start);
134  setGoalState(goal, threshold);
135 }
136 
137 void ompl::base::ProblemDefinition::setGoalState(const State *goal, const double threshold)
138 {
139  clearGoal();
140  GoalState *gs = new GoalState(si_);
141  gs->setState(goal);
142  gs->setThreshold(threshold);
143  setGoal(GoalPtr(gs));
144 }
145 
146 bool ompl::base::ProblemDefinition::hasStartState(const State *state, unsigned int *startIndex)
147 {
148  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
149  if (si_->equalStates(state, startStates_[i]))
150  {
151  if (startIndex)
152  *startIndex = i;
153  return true;
154  }
155  return false;
156 }
157 
158 bool ompl::base::ProblemDefinition::fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
159 {
160  bool result = false;
161 
162  bool b = si_->satisfiesBounds(state);
163  bool v = false;
164  if (b)
165  {
166  v = si_->isValid(state);
167  if (!v)
168  logDebug("%s state is not valid", start ? "Start" : "Goal");
169  }
170  else
171  logDebug("%s state is not within space bounds", start ? "Start" : "Goal");
172 
173  if (!b || !v)
174  {
175  std::stringstream ss;
176  si_->printState(state, ss);
177  ss << " within distance " << dist;
178  logDebug("Attempting to fix %s state %s", start ? "start" : "goal", ss.str().c_str());
179 
180  State *temp = si_->allocState();
181  if (si_->searchValidNearby(temp, state, dist, attempts))
182  {
183  si_->copyState(state, temp);
184  result = true;
185  }
186  else
187  logWarn("Unable to fix %s state", start ? "start" : "goal");
188  si_->freeState(temp);
189  }
190 
191  return result;
192 }
193 
194 bool ompl::base::ProblemDefinition::fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts)
195 {
196  bool result = true;
197 
198  // fix start states
199  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
200  if (!fixInvalidInputState(startStates_[i], distStart, true, attempts))
201  result = false;
202 
203  // fix goal state
204  GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
205  if (goal)
206  {
207  if (!fixInvalidInputState(const_cast<State*>(goal->getState()), distGoal, false, attempts))
208  result = false;
209  }
210 
211  // fix goal state
212  GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
213  if (goals)
214  {
215  for (unsigned int i = 0; i < goals->getStateCount(); ++i)
216  if (!fixInvalidInputState(const_cast<State*>(goals->getState(i)), distGoal, false, attempts))
217  result = false;
218  }
219 
220  return result;
221 }
222 
223 void ompl::base::ProblemDefinition::getInputStates(std::vector<const State*> &states) const
224 {
225  states.clear();
226  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
227  states.push_back(startStates_[i]);
228 
229  GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
230  if (goal)
231  states.push_back(goal->getState());
232 
233  GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
234  if (goals)
235  for (unsigned int i = 0; i < goals->getStateCount(); ++i)
236  states.push_back (goals->getState(i));
237 }
238 
240 {
241  PathPtr path;
242  if (control::SpaceInformationPtr sic = boost::dynamic_pointer_cast<control::SpaceInformation, SpaceInformation>(si_))
243  {
244  unsigned int startIndex;
245  if (isTrivial(&startIndex, NULL))
246  {
248  pc->append(startStates_[startIndex]);
249  control::Control *null = sic->allocControl();
250  sic->nullControl(null);
251  pc->append(startStates_[startIndex], null, 0.0);
252  sic->freeControl(null);
253  path.reset(pc);
254  }
255  else
256  {
257  control::Control *nc = sic->allocControl();
258  State *result1 = sic->allocState();
259  State *result2 = sic->allocState();
260  sic->nullControl(nc);
261 
262  for (unsigned int k = 0 ; k < startStates_.size() && !path ; ++k)
263  {
264  const State *start = startStates_[k];
265  if (start && si_->isValid(start) && si_->satisfiesBounds(start))
266  {
267  sic->copyState(result1, start);
268  for (unsigned int i = 0 ; i < sic->getMaxControlDuration() && !path ; ++i)
269  if (sic->propagateWhileValid(result1, nc, 1, result2))
270  {
271  if (goal_->isSatisfied(result2))
272  {
274  pc->append(start);
275  pc->append(result2, nc, (i + 1) * sic->getPropagationStepSize());
276  path.reset(pc);
277  break;
278  }
279  std::swap(result1, result2);
280  }
281  }
282  }
283  sic->freeState(result1);
284  sic->freeState(result2);
285  sic->freeControl(nc);
286  }
287  }
288  else
289  {
290  std::vector<const State*> states;
291  GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
292  if (goal)
293  if (si_->isValid(goal->getState()) && si_->satisfiesBounds(goal->getState()))
294  states.push_back(goal->getState());
295  GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
296  if (goals)
297  for (unsigned int i = 0; i < goals->getStateCount(); ++i)
298  if (si_->isValid(goals->getState(i)) && si_->satisfiesBounds(goals->getState(i)))
299  states.push_back(goals->getState(i));
300 
301  if (states.empty())
302  {
303  unsigned int startIndex;
304  if (isTrivial(&startIndex))
305  {
306  geometric::PathGeometric *pg = new geometric::PathGeometric(si_, startStates_[startIndex], startStates_[startIndex]);
307  path.reset(pg);
308  }
309  }
310  else
311  {
312  for (unsigned int i = 0 ; i < startStates_.size() && !path ; ++i)
313  {
314  const State *start = startStates_[i];
315  if (start && si_->isValid(start) && si_->satisfiesBounds(start))
316  {
317  for (unsigned int j = 0 ; j < states.size() && !path ; ++j)
318  if (si_->checkMotion(start, states[j]))
319  {
320  geometric::PathGeometric *pg = new geometric::PathGeometric(si_, start, states[j]);
321  path.reset(pg);
322  break;
323  }
324  }
325  }
326  }
327  }
328 
329  return path;
330 }
331 
332 bool ompl::base::ProblemDefinition::isTrivial(unsigned int *startIndex, double *distance) const
333 {
334  if (!goal_)
335  {
336  logError("Goal undefined");
337  return false;
338  }
339 
340  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
341  {
342  const State *start = startStates_[i];
343  if (start && si_->isValid(start) && si_->satisfiesBounds(start))
344  {
345  double dist;
346  if (goal_->isSatisfied(start, &dist))
347  {
348  if (startIndex)
349  *startIndex = i;
350  if (distance)
351  *distance = dist;
352  return true;
353  }
354  }
355  else
356  {
357  logError("Initial state is in collision!");
358  }
359  }
360 
361  return false;
362 }
363 
365 {
366  return solutions_->getSolutionCount() > 0;
367 }
368 
370 {
371  return solutions_->getSolutionCount();
372 }
373 
375 {
376  return solutions_->getTopSolution();
377 }
378 
379 void ompl::base::ProblemDefinition::addSolutionPath(const PathPtr &path, bool approximate, double difference) const
380 {
381  if (approximate)
382  logWarn("Adding approximate solution");
383  solutions_->add(PlannerSolution(path, approximate, difference));
384 }
385 
387 {
388  return solutions_->isApproximate();
389 }
390 
392 {
393  return solutions_->getDifference();
394 }
395 
396 std::vector<ompl::base::PlannerSolution> ompl::base::ProblemDefinition::getSolutions(void) const
397 {
398  return solutions_->getSolutions();
399 }
400 
402 {
403  solutions_->clear();
404 }
405 
406 void ompl::base::ProblemDefinition::print(std::ostream &out) const
407 {
408  out << "Start states:" << std::endl;
409  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
410  si_->printState(startStates_[i], out);
411  if (goal_)
412  goal_->print(out);
413  else
414  out << "Goal = NULL" << std::endl;
415  out << "There are " << solutions_->getSolutionCount() << " solutions" << std::endl;
416 }
417 
418 
420 {
421  return nonExistenceProof_;
422 }
423 
425 {
426  nonExistenceProof_.reset();
427 }
428 
430 {
431  return nonExistenceProof_;
432 }
433 
435 {
436  nonExistenceProof_ = nonExistenceProof;
437 }