Loading...
Searching...
No Matches
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
42ompl::geometric::BiRLRT::BiRLRT(const base::SpaceInformationPtr &si) : base::Planner(si, "BiRLRT")
43{
44 specs_.approximateSolutions = true;
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
53ompl::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();
70
71 if (range_ < 1e-4)
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
98bool 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
126bool 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
154int 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{
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}
Abstract definition of a goal region that can be sampled.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
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...
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...
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...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerInputStates pis_
Utility class to extract valid input states.
Definition Planner.h:416
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
const std::string & getName() const
Get the name of the planner.
Definition Planner.cpp:56
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set,...
Definition Planner.cpp:106
Definition of an abstract state.
Definition State.h:50
A motion (tree node) with parent pointer.
Definition BiRLRT.h:124
const base::State * root
Pointer to the root of the tree this motion is connected to.
Definition BiRLRT.h:140
base::State * state
The state contained by the motion.
Definition BiRLRT.h:134
Motion * parent
The parent motion in the exploration tree.
Definition BiRLRT.h:137
RNG rng_
The random number generator.
Definition BiRLRT.h:178
double range_
The maximum total length of a motion to be added to a tree.
Definition BiRLRT.h:171
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
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
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
double maxDistNear_
The maximum distance (per dimension) when sampling near an existing configuration.
Definition BiRLRT.h:175
base::StateSamplerPtr sampler_
State sampler.
Definition BiRLRT.h:168
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition BiRLRT.cpp:66
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition BiRLRT.h:182
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
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition BiRLRT.cpp:58
void freeMemory()
Free the memory allocated by this planner.
Definition BiRLRT.cpp:78
std::vector< Motion * > tGoal_
Goal tree.
Definition BiRLRT.h:165
std::vector< Motion * > tStart_
Start tree.
Definition BiRLRT.h:163
int connectToTree(const Motion *motion, std::vector< Motion * > &tree)
Definition BiRLRT.cpp:154
Definition of a geometric path.
This class contains methods that automatically configure various parameters for motion planning....
Definition SelfConfig.h:60
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
#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
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.
@ EXACT_SOLUTION
The planner found an exact solution.
@ INVALID_GOAL
Invalid goal state.
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
@ TIMEOUT
The planner failed to find a solution.