RLRT.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, 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 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: Ryan Luna */
36 
37 #include "ompl/geometric/planners/rlrt/RLRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <limits>
41 
42 ompl::geometric::RLRT::RLRT(const base::SpaceInformationPtr &si) : base::Planner(si, "RLRT")
43 {
45  specs_.directed = true;
46 
47  Planner::declareParam<double>("goal_bias", this, &RLRT::setGoalBias, &RLRT::getGoalBias, "0.:.05:1.");
48  Planner::declareParam<double>("range", this, &RLRT::setRange, &RLRT::getRange, "0.:1.:10000.");
49  Planner::declareParam<bool>("keep_last_valid", this, &RLRT::setKeepLast, &RLRT::getKeepLast, "0,1");
50 }
51 
52 ompl::geometric::RLRT::~RLRT()
53 {
54  freeMemory();
55 }
56 
58 {
59  Planner::clear();
60  sampler_.reset();
61  freeMemory();
62  lastGoalMotion_ = nullptr;
63 }
64 
66 {
67  Planner::setup();
68 
69  if (range_ < 1e-4)
70  {
71  tools::SelfConfig sc(si_, getName());
72  sc.configurePlannerRange(range_);
73  }
74 }
75 
77 {
78  for (auto &motion : motions_)
79  {
80  if (motion->state)
81  si_->freeState(motion->state);
82  delete motion;
83  }
84 
85  motions_.clear();
86 }
87 
89 {
90  checkValidity();
91  base::Goal *goal = pdef_->getGoal().get();
92  auto goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
93 
94  while (const base::State *st = pis_.nextStart())
95  {
96  Motion *motion = new Motion(si_);
97  si_->copyState(motion->state, st);
98  motions_.push_back(motion);
99  }
100 
101  if (motions_.size() == 0)
102  {
103  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
105  }
106 
107  if (!sampler_)
108  sampler_ = si_->allocStateSampler();
109 
110  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), motions_.size());
111  if (keepLast_)
112  OMPL_INFORM("%s: keeping last valid state", getName().c_str());
113  else
114  OMPL_INFORM("%s: tree is range limited", getName().c_str());
115 
116  Motion *solution = nullptr;
117  Motion *approxsol = nullptr;
118  double approxdif = std::numeric_limits<double>::infinity();
119  Motion *rmotion = new Motion(si_);
120  base::State *rstate = rmotion->state;
121 
122  std::pair<ompl::base::State *, double> lastValid;
123  lastValid.first = si_->allocState();
124 
125  while (ptc == false)
126  {
127  // Sample a state in the tree uniformly
128  Motion *random = motions_[rng_.uniformInt(0, motions_.size() - 1)];
129 
130  // Sample a random state (with goal biasing)
131  if (goal_s != nullptr && rng_.uniform01() < goalBias_ && goal_s->canSample())
132  goal_s->sampleGoal(rstate);
133  else
134  sampler_->sampleUniform(rstate);
135 
136  Motion *motion = nullptr;
137 
138  if (!keepLast_) // range-limited random tree
139  {
140  // Truncate the distance of the randomly sampled state to range, if necessary
141  double d = si_->distance(random->state, rstate);
142  if (d > range_)
143  si_->getStateSpace()->interpolate(random->state, rstate, range_ / d, rstate);
144 
145  // See if the path between them is valid
146  if (si_->checkMotion(random->state, rstate))
147  {
148  // create a new motion
149  motion = new Motion(si_);
150  si_->copyState(motion->state, rstate);
151  motion->parent = random;
152  motions_.push_back(motion);
153  }
154  }
155  else // keep-last random tree
156  {
157  lastValid.second = 0.0;
158  bool valid = si_->checkMotion(random->state, rstate, lastValid);
159  if (valid || lastValid.second > 1e-3)
160  {
161  // create a new motion
162  motion = new Motion(si_);
163  si_->copyState(motion->state, valid ? rstate : lastValid.first);
164  motion->parent = random;
165  motions_.push_back(motion);
166  }
167  }
168 
169  // we added a state to the tree. See if we're done.
170  if (motion)
171  {
172  double dist = 0.0;
173  bool sat = goal->isSatisfied(motion->state, &dist);
174  if (sat)
175  {
176  approxdif = dist;
177  solution = motion;
178  break;
179  }
180  if (dist < approxdif)
181  {
182  approxdif = dist;
183  approxsol = motion;
184  }
185  }
186  }
187 
188  bool solved = false;
189  bool approximate = false;
190  if (solution == nullptr)
191  {
192  solution = approxsol;
193  approximate = true;
194  }
195 
196  if (solution != nullptr)
197  {
198  lastGoalMotion_ = solution;
199 
200  /* construct the solution path */
201  std::vector<Motion *> mpath;
202  while (solution != nullptr)
203  {
204  mpath.push_back(solution);
205  solution = solution->parent;
206  }
207 
208  /* set the solution path */
209  auto path(std::make_shared<PathGeometric>(si_));
210  for (int i = mpath.size() - 1; i >= 0; --i)
211  path->append(mpath[i]->state);
212  pdef_->addSolutionPath(path, approximate, approxdif, getName());
213  solved = true;
214  }
215 
216  if (rmotion->state)
217  si_->freeState(rmotion->state);
218  delete rmotion;
219  si_->freeState(lastValid.first);
220 
221  OMPL_INFORM("%s: Created %u states", getName().c_str(), motions_.size());
222 
223  return {solved, approximate};
224 }
225 
227 {
228  Planner::getPlannerData(data);
229 
230  if (lastGoalMotion_)
231  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
232 
233  for (auto &motion : motions_)
234  {
235  if (motion->parent == nullptr)
236  data.addStartVertex(base::PlannerDataVertex(motion->state));
237  else
238  data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
239  }
240 }
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
Definition of an abstract state.
Definition: State.h:113
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:123
void setGoalBias(double goalBias)
Set the goal bias In the process of randomly selecting states in the state space to attempt to go tow...
Definition: RLRT.h:180
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RLRT.cpp:65
void setKeepLast(bool keepLast)
Set whether the planner will use the range or keep last heuristic. If keepLast = false,...
Definition: RLRT.h:215
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:486
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:269
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: RLRT.cpp:88
A class to store the exit status of Planner::solve()
Motion * parent
The parent motion in the exploration tree.
Definition: RLRT.h:238
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: RLRT.cpp:57
Abstract definition of goals.
Definition: Goal.h:126
base::State * state
The state contained by the motion.
Definition: RLRT.h:235
void setRange(double distance)
Set the maximum distance between states in the tree.
Definition: RLRT.h:192
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:259
bool getKeepLast() const
If true, the planner will not have the range limitation. Instead, if a collision is detected,...
Definition: RLRT.h:206
A motion (tree node) with parent pointer.
Definition: RLRT.h:224
void freeMemory()
Free the memory allocated by this planner.
Definition: RLRT.cpp:76
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
double getGoalBias() const
Get the goal bias the planner is using.
Definition: RLRT.h:186
Abstract definition of a goal region that can be sampled.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: RLRT.cpp:226
@ INVALID_START
Invalid start state or no start state specified.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
double getRange() const
Get the maximum distance between states in the tree.
Definition: RLRT.h:198