BiRLRT.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/BiRLRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <limits>
41 
42 ompl::geometric::BiRLRT::BiRLRT(const base::SpaceInformationPtr &si) : base::Planner(si, "BiRLRT")
43 {
45  specs_.directed = true;
46 
47  Planner::declareParam<double>("range", this, &BiRLRT::setRange, &BiRLRT::getRange, "0.:1.:10000.");
48  Planner::declareParam<double>("max_dist_near", this, &BiRLRT::setMaxDistanceNear, &BiRLRT::getMaxDistanceNear,
49  "0.:0.1:10.");
50  Planner::declareParam<bool>("keep_last_valid", this, &BiRLRT::setKeepLast, &BiRLRT::getKeepLast, "0,1");
51 }
52 
53 ompl::geometric::BiRLRT::~BiRLRT()
54 {
55  freeMemory();
56 }
57 
59 {
60  Planner::clear();
61  sampler_.reset();
62  freeMemory();
63  connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
64 }
65 
67 {
68  Planner::setup();
69  tools::SelfConfig sc(si_, getName());
70 
71  if (range_ < 1e-4)
72  sc.configurePlannerRange(range_);
73 
74  if (maxDistNear_ < 1e-4)
75  maxDistNear_ = range_ / 20.0; // make this pretty small
76 }
77 
79 {
80  for (auto &motion : tStart_)
81  {
82  if (motion->state != nullptr)
83  si_->freeState(motion->state);
84  delete motion;
85  }
86  for (auto &motion : tGoal_)
87  {
88  if (motion->state != nullptr)
89  si_->freeState(motion->state);
90  delete motion;
91  }
92 
93  tStart_.clear();
94  tGoal_.clear();
95 }
96 
98 bool ompl::geometric::BiRLRT::growTreeRangeLimited(std::vector<Motion *> &tree, Motion *xmotion)
99 {
100  assert(tree.size() > 0);
101 
102  // select a state from tree to expand from
103  Motion *randomMotion = tree[rng_.uniformInt(0, tree.size() - 1)];
104 
105  // Sample a random direction. Limit length of motion to range_, if necessary
106  sampler_->sampleUniform(xmotion->state);
107  double d = si_->distance(randomMotion->state, xmotion->state);
108  if (d > range_)
109  si_->getStateSpace()->interpolate(randomMotion->state, xmotion->state, range_ / d, xmotion->state);
110 
111  if (si_->checkMotion(randomMotion->state, xmotion->state))
112  {
113  Motion *motion = new Motion(si_);
114  si_->copyState(motion->state, xmotion->state);
115  motion->parent = randomMotion;
116  motion->root = randomMotion->root;
117 
118  tree.push_back(motion);
119  return true;
120  }
121 
122  return false;
123 }
124 
126 bool ompl::geometric::BiRLRT::growTreeKeepLast(std::vector<Motion *> &tree, Motion *xmotion,
127  std::pair<ompl::base::State *, double> &lastValid)
128 {
129  assert(tree.size() > 0);
130 
131  // select a state from tree to expand from
132  Motion *randomMotion = tree[rng_.uniformInt(0, tree.size() - 1)];
133 
134  // Sample a random state.
135  sampler_->sampleUniform(xmotion->state);
136 
137  lastValid.second = 0.0;
138  bool valid = si_->checkMotion(randomMotion->state, xmotion->state, lastValid);
139  if (valid || lastValid.second > 1e-3)
140  {
141  // create a new motion
142  Motion *motion = new Motion(si_);
143  si_->copyState(motion->state, valid ? xmotion->state : lastValid.first);
144  motion->parent = randomMotion;
145  motion->root = randomMotion->root;
146 
147  tree.push_back(motion);
148  return true;
149  }
150 
151  return false;
152 }
153 
154 int ompl::geometric::BiRLRT::connectToTree(const Motion *motion, std::vector<Motion *> &tree)
155 {
156  assert(tree.size() > 0);
157 
158  int checks = tree.size() > 1 ? ceil(log(tree.size())) : 1;
159 
160  for (int i = 0; i < checks; ++i)
161  {
162  // select a state from tree to expand from
163  int randIndex = rng_.uniformInt(0, tree.size() - 1);
164  Motion *randomMotion = tree[randIndex];
165 
166  if (si_->checkMotion(randomMotion->state, motion->state))
167  return randIndex;
168  }
169 
170  return -1; // failed connection
171 }
172 
174 {
175  checkValidity();
176  auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
177  if (!goal)
178  {
179  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
181  }
182 
183  while (const base::State *st = pis_.nextStart())
184  {
185  Motion *motion = new Motion(si_);
186  si_->copyState(motion->state, st);
187  motion->root = motion->state;
188  tStart_.push_back(motion);
189  }
190 
191  if (tStart_.size() == 0)
192  {
193  OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
195  }
196 
197  if (!sampler_)
198  sampler_ = si_->allocStateSampler();
199 
200  if (!goal->couldSample())
201  {
202  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
204  }
205 
206  OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
207  (int)(tStart_.size() + tGoal_.size()));
208  if (keepLast_)
209  OMPL_INFORM("%s: keeping last valid state", getName().c_str());
210  else
211  OMPL_INFORM("%s: tree is range limited", getName().c_str());
212 
213  std::vector<Motion *> *tree, *otherTree;
214  tree = &tStart_;
215  otherTree = &tGoal_;
216  bool solved = false;
217 
218  auto xmotion = new Motion(si_);
219 
220  std::pair<ompl::base::State *, double> lastValid;
221  lastValid.first = si_->allocState();
222 
223  while (ptc == false && solved == false)
224  {
225  if (tGoal_.size() == 0 || pis_.getSampledGoalsCount() < tGoal_.size() / 2)
226  {
227  const base::State *st = tGoal_.size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
228  if (st)
229  {
230  Motion *motion = new Motion(si_);
231  si_->copyState(motion->state, st);
232  motion->root = motion->state;
233  tGoal_.push_back(motion);
234  }
235 
236  if (tGoal_.size() == 0)
237  {
238  OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
239  break;
240  }
241  }
242 
243  bool expanded = keepLast_ ? growTreeKeepLast(*tree, xmotion, lastValid) : growTreeRangeLimited(*tree, xmotion);
244  if (expanded)
245  {
246  int connectionIdx = connectToTree(tree->back(), *otherTree);
247  if (connectionIdx >= 0)
248  {
249  // there is a solution path. construct it.
250  Motion *startMotion = tree == &tStart_ ? tree->back() : otherTree->at(connectionIdx);
251  Motion *goalMotion = tree == &tStart_ ? otherTree->at(connectionIdx) : tree->back();
252 
253  // start and goal pair is not valid for this problem
254  if (!goal->isStartGoalPairValid(startMotion->root, goalMotion->root))
255  continue;
256 
257  connectionPoint_ = std::make_pair(startMotion->state, goalMotion->state);
258 
259  Motion *solution = startMotion;
260  std::vector<Motion *> mpath1;
261  while (solution != nullptr)
262  {
263  mpath1.push_back(solution);
264  solution = solution->parent;
265  }
266 
267  solution = goalMotion;
268  std::vector<Motion *> mpath2;
269  while (solution != nullptr)
270  {
271  mpath2.push_back(solution);
272  solution = solution->parent;
273  }
274 
275  PathGeometric *path = new PathGeometric(si_);
276  path->getStates().reserve(mpath1.size() + mpath2.size());
277  for (int i = mpath1.size() - 1; i >= 0; --i)
278  path->append(mpath1[i]->state);
279  for (unsigned int i = 0; i < mpath2.size(); ++i)
280  path->append(mpath2[i]->state);
281 
282  pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName());
283  solved = true;
284  }
285  }
286  std::swap(tree, otherTree);
287  }
288 
289  si_->freeState(xmotion->state);
290  delete xmotion;
291  si_->freeState(lastValid.first);
292 
293  OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_.size() + tGoal_.size(),
294  tStart_.size(), tGoal_.size());
295 
297 }
298 
300 {
301  Planner::getPlannerData(data);
302 
303  for (auto &motion : tStart_)
304  {
305  if (motion->parent == nullptr)
306  data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
307  else
308  {
309  data.addEdge(base::PlannerDataVertex(motion->parent->state, 1), base::PlannerDataVertex(motion->state, 1));
310  }
311  }
312  for (auto &motion : tGoal_)
313  {
314  if (motion->parent == nullptr)
315  data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
316  else
317  {
318  data.addEdge(base::PlannerDataVertex(motion->parent->state, 1), base::PlannerDataVertex(motion->state, 1));
319  }
320  }
321 
322  // Add the edge connecting the two trees
323  data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
324 }
bool getKeepLast() const
If true, the planner will not have the range limitation. Instead, if a collision is detected,...
Definition: BiRLRT.h:201
Motion * parent
The parent motion in the exploration tree.
Definition: BiRLRT.h:233
void freeMemory()
Free the memory allocated by this planner.
Definition: BiRLRT.cpp:78
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
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:114
int connectToTree(const Motion *motion, std::vector< Motion * > &tree)
Definition: BiRLRT.cpp:154
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:124
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BiRLRT.cpp:58
void setKeepLast(bool keepLast)
Set whether the planner will use the range or keep last heuristic. If keepLast = false,...
Definition: BiRLRT.h:210
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BiRLRT.cpp:66
A motion (tree node) with parent pointer.
Definition: BiRLRT.h:220
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
@ TIMEOUT
The planner failed to find a solution.
void setRange(double distance)
Set the maximum distance between states in the tree.
Definition: BiRLRT.h:173
Definition of a geometric path.
Definition: PathGeometric.h:98
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:239
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: BiRLRT.cpp:173
const base::State * root
Pointer to the root of the tree this motion is connected to.
Definition: BiRLRT.h:236
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:493
@ INVALID_GOAL
Invalid goal state.
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:276
double getMaxDistanceNear() const
Get the maximum distance (per dimension) when sampling near an existing state.
Definition: BiRLRT.h:193
base::State * state
The state contained by the motion.
Definition: BiRLRT.h:230
A class to store the exit status of Planner::solve()
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: BiRLRT.cpp:299
void setMaxDistanceNear(double dNear)
Set the maximum distance (per dimension) when sampling near an existing state.
Definition: BiRLRT.h:186
double getRange() const
Get the maximum distance between states in the tree.
Definition: BiRLRT.h:179
@ EXACT_SOLUTION
The planner found an exact solution.
bool growTreeKeepLast(std::vector< Motion * > &tree, Motion *xmotion, std::pair< base::State *, double > &lastValid)
Try to grow the tree randomly. Return true if a new state was added.
Definition: BiRLRT.cpp:126
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...
bool growTreeRangeLimited(std::vector< Motion * > &tree, Motion *xmotion)
Try to grow the tree randomly. Return true if a new state was added.
Definition: BiRLRT.cpp:98
#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:266
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...
Abstract definition of a goal region that can be sampled.
@ 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:123