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