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 
168 void ompl::base::ProblemDefinition::setStartAndGoalStates(const State *start, const State *goal, const double threshold)
169 {
171  addStartState(start);
172  setGoalState(goal, threshold);
173 }
174 
175 void ompl::base::ProblemDefinition::setGoalState(const State *goal, const double threshold)
176 {
177  clearGoal();
178  auto gs(std::make_shared<GoalState>(si_));
179  gs->setState(goal);
180  gs->setThreshold(threshold);
181  setGoal(gs);
182 }
183 
184 bool ompl::base::ProblemDefinition::hasStartState(const State *state, unsigned int *startIndex) const
185 {
186  for (unsigned int i = 0; i < startStates_.size(); ++i)
187  if (si_->equalStates(state, startStates_[i]))
188  {
189  if (startIndex)
190  *startIndex = i;
191  return true;
192  }
193  return false;
194 }
195 
196 bool ompl::base::ProblemDefinition::fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
197 {
198  bool result = false;
199 
200  bool b = si_->satisfiesBounds(state);
201  bool v = false;
202  if (b)
203  {
204  v = si_->isValid(state);
205  if (!v)
206  OMPL_DEBUG("%s state is not valid", start ? "Start" : "Goal");
207  }
208  else
209  OMPL_DEBUG("%s state is not within space bounds", start ? "Start" : "Goal");
210 
211  if (!b || !v)
212  {
213  std::stringstream ss;
214  si_->printState(state, ss);
215  ss << " within distance " << dist;
216  OMPL_DEBUG("Attempting to fix %s state %s", start ? "start" : "goal", ss.str().c_str());
217 
218  State *temp = si_->allocState();
219  if (si_->searchValidNearby(temp, state, dist, attempts))
220  {
221  si_->copyState(state, temp);
222  result = true;
223  }
224  else
225  OMPL_WARN("Unable to fix %s state", start ? "start" : "goal");
226  si_->freeState(temp);
227  }
228 
229  return result;
230 }
231 
232 bool ompl::base::ProblemDefinition::fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts)
233 {
234  bool result = true;
235 
236  // fix start states
237  for (auto &startState : startStates_)
238  if (!fixInvalidInputState(startState, distStart, true, attempts))
239  result = false;
240 
241  // fix goal state
242  auto *goal = dynamic_cast<GoalState *>(goal_.get());
243  if (goal)
244  {
245  if (!fixInvalidInputState(const_cast<State *>(goal->getState()), distGoal, false, attempts))
246  result = false;
247  }
248 
249  // fix goal state
250  auto *goals = dynamic_cast<GoalStates *>(goal_.get());
251  if (goals)
252  {
253  for (unsigned int i = 0; i < goals->getStateCount(); ++i)
254  if (!fixInvalidInputState(const_cast<State *>(goals->getState(i)), distGoal, false, attempts))
255  result = false;
256  }
257 
258  return result;
259 }
260 
261 void ompl::base::ProblemDefinition::getInputStates(std::vector<const State *> &states) const
262 {
263  states.clear();
264  for (auto startState : startStates_)
265  states.push_back(startState);
266 
267  auto *goal = dynamic_cast<GoalState *>(goal_.get());
268  if (goal)
269  states.push_back(goal->getState());
270 
271  auto *goals = dynamic_cast<GoalStates *>(goal_.get());
272  if (goals)
273  for (unsigned int i = 0; i < goals->getStateCount(); ++i)
274  states.push_back(goals->getState(i));
275 }
276 
278 {
279  PathPtr path;
280  if (control::SpaceInformationPtr sic = std::dynamic_pointer_cast<control::SpaceInformation, SpaceInformation>(si_))
281  {
282  unsigned int startIndex;
283  if (isTrivial(&startIndex, nullptr))
284  {
285  auto pc(std::make_shared<control::PathControl>(sic));
286  pc->append(startStates_[startIndex]);
287  control::Control *null = sic->allocControl();
288  sic->nullControl(null);
289  pc->append(startStates_[startIndex], null, 0.0);
290  sic->freeControl(null);
291  path = pc;
292  }
293  else
294  {
295  control::Control *nc = sic->allocControl();
296  State *result1 = sic->allocState();
297  State *result2 = sic->allocState();
298  sic->nullControl(nc);
299 
300  for (unsigned int k = 0; k < startStates_.size() && !path; ++k)
301  {
302  const State *start = startStates_[k];
303  if (start && si_->isValid(start) && si_->satisfiesBounds(start))
304  {
305  sic->copyState(result1, start);
306  for (unsigned int i = 0; i < sic->getMaxControlDuration() && !path; ++i)
307  if (sic->propagateWhileValid(result1, nc, 1, result2))
308  {
309  if (goal_->isSatisfied(result2))
310  {
311  auto pc(std::make_shared<control::PathControl>(sic));
312  pc->append(start);
313  pc->append(result2, nc, (i + 1) * sic->getPropagationStepSize());
314  path = pc;
315  break;
316  }
317  std::swap(result1, result2);
318  }
319  }
320  }
321  sic->freeState(result1);
322  sic->freeState(result2);
323  sic->freeControl(nc);
324  }
325  }
326  else
327  {
328  std::vector<const State *> states;
329  auto *goal = dynamic_cast<GoalState *>(goal_.get());
330  if (goal)
331  if (si_->isValid(goal->getState()) && si_->satisfiesBounds(goal->getState()))
332  states.push_back(goal->getState());
333  auto *goals = dynamic_cast<GoalStates *>(goal_.get());
334  if (goals)
335  for (unsigned int i = 0; i < goals->getStateCount(); ++i)
336  if (si_->isValid(goals->getState(i)) && si_->satisfiesBounds(goals->getState(i)))
337  states.push_back(goals->getState(i));
338 
339  if (states.empty())
340  {
341  unsigned int startIndex;
342  if (isTrivial(&startIndex))
343  path =
344  std::make_shared<geometric::PathGeometric>(si_, startStates_[startIndex], startStates_[startIndex]);
345  }
346  else
347  {
348  for (unsigned int i = 0; i < startStates_.size() && !path; ++i)
349  {
350  const State *start = startStates_[i];
351  if (start && si_->isValid(start) && si_->satisfiesBounds(start))
352  {
353  for (unsigned int j = 0; j < states.size() && !path; ++j)
354  if (si_->checkMotion(start, states[j]))
355  {
356  path = std::make_shared<geometric::PathGeometric>(si_, start, states[j]);
357  break;
358  }
359  }
360  }
361  }
362  }
363 
364  return path;
365 }
366 
367 bool ompl::base::ProblemDefinition::isTrivial(unsigned int *startIndex, double *distance) const
368 {
369  if (!goal_)
370  {
371  OMPL_ERROR("Goal undefined");
372  return false;
373  }
374 
375  for (unsigned int i = 0; i < startStates_.size(); ++i)
376  {
377  const State *start = startStates_[i];
378  if (start && si_->isValid(start) && si_->satisfiesBounds(start))
379  {
380  double dist;
381  if (goal_->isSatisfied(start, &dist))
382  {
383  if (startIndex)
384  *startIndex = i;
385  if (distance)
386  *distance = dist;
387  return true;
388  }
389  }
390  else
391  {
392  OMPL_ERROR("Initial state is in collision!");
393  }
394  }
395 
396  return false;
397 }
398 
400 {
401  return solutions_->getSolutionCount() > 0;
402 }
403 
405 {
406  return solutions_->getSolutionCount();
407 }
408 
410 {
411  return solutions_->getTopSolution();
412 }
413 
415 {
416  return solutions_->getTopSolution(solution);
417 }
418 
419 void ompl::base::ProblemDefinition::addSolutionPath(const PathPtr &path, bool approximate, double difference,
420  const std::string &plannerName) const
421 {
422  PlannerSolution sol(path);
423  if (approximate)
424  sol.setApproximate(difference);
425  sol.setPlannerName(plannerName);
426  addSolutionPath(sol);
427 }
428 
430 {
431  if (sol.approximate_)
432  OMPL_INFORM("ProblemDefinition: Adding approximate solution from planner %s", sol.plannerName_.c_str());
433  solutions_->add(sol);
434 }
435 
437 {
438  return solutions_->isApproximate();
439 }
440 
442 {
443  return solutions_->isOptimized();
444 }
445 
447 {
448  return solutions_->getDifference();
449 }
450 
451 std::vector<ompl::base::PlannerSolution> ompl::base::ProblemDefinition::getSolutions() const
452 {
453  return solutions_->getSolutions();
454 }
455 
457 {
458  solutions_->clear();
459 }
460 
461 void ompl::base::ProblemDefinition::print(std::ostream &out) const
462 {
463  out << "Start states:" << std::endl;
464  for (auto startState : startStates_)
465  si_->printState(startState, out);
466  if (goal_)
467  goal_->print(out);
468  else
469  out << "Goal = nullptr" << std::endl;
471  {
472  optimizationObjective_->print(out);
473  out << "Average state cost: " << optimizationObjective_->averageStateCost(magic::TEST_STATE_COUNT) << std::endl;
474  }
475  else
476  out << "OptimizationObjective = nullptr" << 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  const ompl::base::SolutionNonExistenceProofPtr &nonExistenceProof)
497 {
498  nonExistenceProof_ = nonExistenceProof;
499 }
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 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.
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.
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.
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.
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.
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 ...
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.
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