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  = default;
59 
60  void add(const PlannerSolution &s)
61  {
62  std::lock_guard<std::mutex> slock(lock_);
63  int index = solutions_.size();
64  solutions_.push_back(s);
65  solutions_.back().index_ = index;
66  std::sort(solutions_.begin(), solutions_.end());
67  }
68 
69  void clear()
70  {
71  std::lock_guard<std::mutex> slock(lock_);
72  solutions_.clear();
73  }
74 
75  std::vector<PlannerSolution> getSolutions()
76  {
77  std::lock_guard<std::mutex> slock(lock_);
78  std::vector<PlannerSolution> copy = solutions_;
79  return copy;
80  }
81 
82  bool isApproximate()
83  {
84  std::lock_guard<std::mutex> slock(lock_);
85  bool result = false;
86  if (!solutions_.empty())
87  result = solutions_[0].approximate_;
88  return result;
89  }
90 
91  bool isOptimized()
92  {
93  std::lock_guard<std::mutex> slock(lock_);
94  bool result = false;
95  if (!solutions_.empty())
96  result = solutions_[0].optimized_;
97  return result;
98  }
99 
100  double getDifference()
101  {
102  std::lock_guard<std::mutex> slock(lock_);
103  double diff = -1.0;
104  if (!solutions_.empty())
105  diff = solutions_[0].difference_;
106  return diff;
107  }
108 
109  PathPtr getTopSolution()
110  {
111  std::lock_guard<std::mutex> slock(lock_);
112  PathPtr copy;
113  if (!solutions_.empty())
114  copy = solutions_[0].path_;
115  return copy;
116  }
117 
118  bool getTopSolution(PlannerSolution &solution)
119  {
120  std::lock_guard<std::mutex> slock(lock_);
121 
122  if (!solutions_.empty())
123  {
124  solution = solutions_[0];
125  return true;
126  }
127  else
128  {
129  return false;
130  }
131  }
132 
133  std::size_t getSolutionCount()
134  {
135  std::lock_guard<std::mutex> slock(lock_);
136  std::size_t result = solutions_.size();
137  return result;
138  }
139 
140  private:
141  std::vector<PlannerSolution> solutions_;
142  std::mutex lock_;
143  };
144  }
145 }
147 
149 {
150  if (!approximate_ && b.approximate_)
151  return true;
152  if (approximate_ && !b.approximate_)
153  return false;
154  if (approximate_ && b.approximate_)
155  return difference_ < b.difference_;
156  if (optimized_ && !b.optimized_)
157  return true;
158  if (!optimized_ && b.optimized_)
159  return false;
160  return opt_ ? opt_->isCostBetterThan(cost_, b.cost_) : length_ < b.length_;
161 }
162 
163 ompl::base::ProblemDefinition::ProblemDefinition(SpaceInformationPtr si)
164  : si_(std::move(si)), solutions_(std::make_shared<PlannerSolutionSet>())
165 {
166 }
167 
169 {
170  auto result = std::make_shared<ProblemDefinition>(si_);
171  result->startStates_.reserve(startStates_.size());
172  for (const auto &state : startStates_)
173  result->addStartState(state);
174  result->setGoal(goal_);
175  result->setOptimizationObjective(optimizationObjective_);
176  result->setSolutionNonExistenceProof(nonExistenceProof_);
177 
178  return result;
179 }
180 
181 void ompl::base::ProblemDefinition::setStartAndGoalStates(const State *start, const State *goal, const double threshold)
182 {
183  clearStartStates();
184  addStartState(start);
185  setGoalState(goal, threshold);
186 }
187 
188 void ompl::base::ProblemDefinition::setGoalState(const State *goal, const double threshold)
189 {
190  clearGoal();
191  auto gs(std::make_shared<GoalState>(si_));
192  gs->setState(goal);
193  gs->setThreshold(threshold);
194  setGoal(gs);
195 }
196 
197 bool ompl::base::ProblemDefinition::hasStartState(const State *state, unsigned int *startIndex) const
198 {
199  for (unsigned int i = 0; i < startStates_.size(); ++i)
200  if (si_->equalStates(state, startStates_[i]))
201  {
202  if (startIndex)
203  *startIndex = i;
204  return true;
205  }
206  return false;
207 }
208 
209 bool ompl::base::ProblemDefinition::fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
210 {
211  bool result = false;
212 
213  bool b = si_->satisfiesBounds(state);
214  bool v = false;
215  if (b)
216  {
217  v = si_->isValid(state);
218  if (!v)
219  OMPL_DEBUG("%s state is not valid", start ? "Start" : "Goal");
220  }
221  else
222  OMPL_DEBUG("%s state is not within space bounds", start ? "Start" : "Goal");
223 
224  if (!b || !v)
225  {
226  std::stringstream ss;
227  si_->printState(state, ss);
228  ss << " within distance " << dist;
229  OMPL_DEBUG("Attempting to fix %s state %s", start ? "start" : "goal", ss.str().c_str());
230 
231  State *temp = si_->allocState();
232  if (si_->searchValidNearby(temp, state, dist, attempts))
233  {
234  si_->copyState(state, temp);
235  result = true;
236  }
237  else
238  OMPL_WARN("Unable to fix %s state", start ? "start" : "goal");
239  si_->freeState(temp);
240  }
241 
242  return result;
243 }
244 
245 bool ompl::base::ProblemDefinition::fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts)
246 {
247  bool result = true;
248 
249  // fix start states
250  for (auto &startState : startStates_)
251  if (!fixInvalidInputState(startState, distStart, true, attempts))
252  result = false;
253 
254  // fix goal state
255  auto *goal = dynamic_cast<GoalState *>(goal_.get());
256  if (goal)
257  {
258  if (!fixInvalidInputState(const_cast<State *>(goal->getState()), distGoal, false, attempts))
259  result = false;
260  }
261 
262  // fix goal state
263  auto *goals = dynamic_cast<GoalStates *>(goal_.get());
264  if (goals)
265  {
266  for (unsigned int i = 0; i < goals->getStateCount(); ++i)
267  if (!fixInvalidInputState(const_cast<State *>(goals->getState(i)), distGoal, false, attempts))
268  result = false;
269  }
270 
271  return result;
272 }
273 
274 void ompl::base::ProblemDefinition::getInputStates(std::vector<const State *> &states) const
275 {
276  states.clear();
277  for (auto startState : startStates_)
278  states.push_back(startState);
279 
280  auto *goal = dynamic_cast<GoalState *>(goal_.get());
281  if (goal)
282  states.push_back(goal->getState());
283 
284  auto *goals = dynamic_cast<GoalStates *>(goal_.get());
285  if (goals)
286  for (unsigned int i = 0; i < goals->getStateCount(); ++i)
287  states.push_back(goals->getState(i));
288 }
289 
291 {
292  PathPtr path;
293  if (control::SpaceInformationPtr sic = std::dynamic_pointer_cast<control::SpaceInformation, SpaceInformation>(si_))
294  {
295  unsigned int startIndex;
296  if (isTrivial(&startIndex, nullptr))
297  {
298  auto pc(std::make_shared<control::PathControl>(sic));
299  pc->append(startStates_[startIndex]);
300  control::Control *null = sic->allocControl();
301  sic->nullControl(null);
302  pc->append(startStates_[startIndex], null, 0.0);
303  sic->freeControl(null);
304  path = pc;
305  }
306  else
307  {
308  control::Control *nc = sic->allocControl();
309  State *result1 = sic->allocState();
310  State *result2 = sic->allocState();
311  sic->nullControl(nc);
312 
313  for (unsigned int k = 0; k < startStates_.size() && !path; ++k)
314  {
315  const State *start = startStates_[k];
316  if (start && si_->isValid(start) && si_->satisfiesBounds(start))
317  {
318  sic->copyState(result1, start);
319  for (unsigned int i = 0; i < sic->getMaxControlDuration() && !path; ++i)
320  if (sic->propagateWhileValid(result1, nc, 1, result2))
321  {
322  if (goal_->isSatisfied(result2))
323  {
324  auto pc(std::make_shared<control::PathControl>(sic));
325  pc->append(start);
326  pc->append(result2, nc, (i + 1) * sic->getPropagationStepSize());
327  path = pc;
328  break;
329  }
330  std::swap(result1, result2);
331  }
332  }
333  }
334  sic->freeState(result1);
335  sic->freeState(result2);
336  sic->freeControl(nc);
337  }
338  }
339  else
340  {
341  std::vector<const State *> states;
342  auto *goal = dynamic_cast<GoalState *>(goal_.get());
343  if (goal)
344  if (si_->isValid(goal->getState()) && si_->satisfiesBounds(goal->getState()))
345  states.push_back(goal->getState());
346  auto *goals = dynamic_cast<GoalStates *>(goal_.get());
347  if (goals)
348  for (unsigned int i = 0; i < goals->getStateCount(); ++i)
349  if (si_->isValid(goals->getState(i)) && si_->satisfiesBounds(goals->getState(i)))
350  states.push_back(goals->getState(i));
351 
352  if (states.empty())
353  {
354  unsigned int startIndex;
355  if (isTrivial(&startIndex))
356  path =
357  std::make_shared<geometric::PathGeometric>(si_, startStates_[startIndex], startStates_[startIndex]);
358  }
359  else
360  {
361  for (const auto start : startStates_)
362  if (start && si_->isValid(start) && si_->satisfiesBounds(start))
363  for (const auto state : states)
364  if (si_->checkMotion(start, state))
365  return std::make_shared<geometric::PathGeometric>(si_, start, state);
366  }
367  }
368 
369  return path;
370 }
371 
372 bool ompl::base::ProblemDefinition::isTrivial(unsigned int *startIndex, double *distance) const
373 {
374  if (!goal_)
375  {
376  OMPL_ERROR("Goal undefined");
377  return false;
378  }
379 
380  for (unsigned int i = 0; i < startStates_.size(); ++i)
381  {
382  const State *start = startStates_[i];
383  if (start && si_->isValid(start) && si_->satisfiesBounds(start))
384  {
385  double dist;
386  if (goal_->isSatisfied(start, &dist))
387  {
388  if (startIndex)
389  *startIndex = i;
390  if (distance)
391  *distance = dist;
392  return true;
393  }
394  }
395  else
396  {
397  OMPL_ERROR("Initial state is in collision!");
398  }
399  }
400 
401  return false;
402 }
403 
405 {
406  return solutions_->getSolutionCount() > 0;
407 }
408 
410 {
411  return solutions_->getSolutionCount();
412 }
413 
415 {
416  return solutions_->getTopSolution();
417 }
418 
420 {
421  return solutions_->getTopSolution(solution);
422 }
423 
424 void ompl::base::ProblemDefinition::addSolutionPath(const PathPtr &path, bool approximate, double difference,
425  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 (auto startState : startStates_)
470  si_->printState(startState, out);
471  if (goal_)
472  goal_->print(out);
473  else
474  out << "Goal = nullptr" << std::endl;
475  if (optimizationObjective_)
476  {
477  optimizationObjective_->print(out);
478  out << "Average state cost: " << optimizationObjective_->averageStateCost(magic::TEST_STATE_COUNT) << std::endl;
479  }
480  else
481  out << "OptimizationObjective = nullptr" << std::endl;
482  out << "There are " << solutions_->getSolutionCount() << " solutions" << std::endl;
483 }
484 
486 {
487  return nonExistenceProof_.get();
488 }
489 
491 {
492  nonExistenceProof_.reset();
493 }
494 
496 {
497  return nonExistenceProof_;
498 }
499 
501  const ompl::base::SolutionNonExistenceProofPtr &nonExistenceProof)
502 {
503  nonExistenceProof_ = nonExistenceProof;
504 }
Definition of a goal state.
Definition: GoalState.h:49
Definition of a set of goal states.
Definition: GoalStates.h:50
A shared pointer wrapper for ompl::base::Path.
A shared pointer wrapper for ompl::base::ProblemDefinition.
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...
void setGoalState(const State *goal, 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 clearSolutionNonExistenceProof()
Removes any existing instance of SolutionNonExistenceProof.
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...
std::size_t getSolutionCount() const
Get the number of solutions already found.
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...
bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
Helper function for fixInvalidInputStates(). Attempts to fix an individual state.
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 std::string &plannerName="Unknown") const
Add a solution path in a thread-safe manner. Multiple solutions can be set for a goal....
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 setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr &nonExistenceProof)
Set the instance of SolutionNonExistenceProof for this problem definition.
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 hasApproximateSolution() const
Return true if the top found solution is approximate (does not actually reach the desired goal,...
bool hasSolutionNonExistenceProof() const
Returns true if the problem definition has a proof of non existence for a solution.
bool hasSolution() const
Returns true if a solution path has been found (could be approximate)
bool getSolution(PlannerSolution &solution) const
Return true if a top solution is found, with the top solution passed by reference in the function hea...
PathPtr getSolutionPath() const
Return the top solution path, if one is found. The top path is the shortest one that was found,...
const SolutionNonExistenceProofPtr & getSolutionNonExistenceProof() const
Retrieve a pointer to the SolutionNonExistenceProof instance for this problem definition.
void setStartAndGoalStates(const State *start, const State *goal, double threshold=std::numeric_limits< double >::epsilon())
In the simplest case possible, we have a single starting state and a single goal state.
std::vector< PlannerSolution > getSolutions() const
Get all the solution paths available for this goal.
bool hasOptimizedSolution() const
Return true if the top found solution is optimized (satisfies the specified optimization objective)
void clearSolutionPaths() const
Forget the solution paths (thread safe). Memory is freed.
ProblemDefinitionPtr clone() const
Return a copy of the problem definition.
double getSolutionDifference() const
Get the distance to the desired goal for the top solution. Return -1.0 if there are no solutions avai...
A shared pointer wrapper for ompl::base::SolutionNonExistenceProof.
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
Definition of an abstract control.
Definition: Control.h:48
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
Main namespace. Contains everything in this library.
Representation of a solution to a planning problem.
bool optimized_
True if the solution was optimized to meet the specified optimization criterion.
std::string plannerName_
Name of planner type that generated this solution, as received from Planner::getName()
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
Cost cost_
The cost of this solution path, with respect to the optimization objective.
OptimizationObjectivePtr opt_
Optimization objective that was used to optimize this solution.
bool approximate_
True if goal was not achieved, but an approximate solution was found.
double length_
For efficiency reasons, keep the length of the path as well.
double difference_
The achieved difference between the found solution and the desired goal.
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
bool operator<(const PlannerSolution &b) const
Define a ranking for solutions.