Loading...
Searching...
No Matches
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
50namespace 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
163ompl::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
181void 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
188void 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
197bool 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
209bool 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
245bool 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
274void 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
372bool 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
424void 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
456std::vector<ompl::base::PlannerSolution> ompl::base::ProblemDefinition::getSolutions() const
457{
458 return solutions_->getSolutions();
459}
460
462{
463 solutions_->clear();
464}
465
466void 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.
STL namespace.
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.