Loading...
Searching...
No Matches
QRRTStarImpl.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2020,
5 * Max Planck Institute for Intelligent Systems (MPI-IS).
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the MPI-IS nor the names
19 * of its contributors may be used to endorse or promote products
20 * derived from this software without specific prior written
21 * permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *********************************************************************/
36
37/* Author: Andreas Orthey, Sohaib Akbar */
38
39#include <ompl/multilevel/planners/qrrt/QRRTStarImpl.h>
40#include <ompl/multilevel/datastructures/PlannerDataVertexAnnotated.h>
41#include <ompl/tools/config/SelfConfig.h>
42#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
43#include <boost/foreach.hpp>
44#include <boost/math/constants/constants.hpp>
45#include "ompl/util/GeometricEquations.h"
46#include <memory>
47
48#define foreach BOOST_FOREACH
49
50ompl::multilevel::QRRTStarImpl::QRRTStarImpl(const base::SpaceInformationPtr &si, BundleSpace *parent_)
51 : BaseT(si, parent_)
52{
53 setName("QRRTStarImpl" + std::to_string(id_));
54 Planner::declareParam<double>("useKNearest_", this, &ompl::multilevel::QRRTStarImpl::setKNearest,
55 &ompl::multilevel::QRRTStarImpl::getKNearest, "0,1");
56
57 symmetric_ = getBundle()->getStateSpace()->hasSymmetricInterpolate();
58
59 setImportance("exponential");
60 // setGraphSampler("randomedge");
61 setGraphSampler("randomvertex");
62 setMetric("geodesic");
63}
64
65ompl::multilevel::QRRTStarImpl::~QRRTStarImpl()
66{
67}
68
69void ompl::multilevel::QRRTStarImpl::setup()
70{
71 BaseT::setup();
72 calculateRewiringLowerBounds();
73}
74void ompl::multilevel::QRRTStarImpl::clear()
75{
76 BaseT::clear();
77 goalConfigurations_.clear();
78}
79
81{
82 if (firstRun_)
83 {
84 init();
85 firstRun_ = false;
86
88 }
89
90 //(1) Get Random Sample
91 sampleBundleGoalBias(xRandom_->state);
92
93 //(2) Get Nearest in Tree
94 Configuration *q_nearest = nearestDatastructure_->nearest(xRandom_);
95
96 //(3) Steer toward random
97 Configuration *q_new = steerTowards_Range(q_nearest, xRandom_);
98
99 if (q_new)
100 {
101 // (1) Find all neighbors of the new configuration in graph
102 std::vector<Configuration *> nearestNbh;
103 getNearestNeighbors(q_new, nearestNbh);
104
105 // (2) Find neighbor with minimum Cost
106 q_new->lineCost = getOptimizationObjectivePtr()->motionCost(q_nearest->state, q_new->state);
107 q_new->cost = getOptimizationObjectivePtr()->combineCosts(q_nearest->cost, q_new->lineCost);
108 q_new->parent = q_nearest;
109
110 // (3) Rewire Tree
111 base::Cost cost_nearest = q_new->cost;
112
113 std::vector<int> validNeighbor(nearestNbh.size(), 0);
114
115 // store the connection cost for later use, if space is symmetric
116 std::vector<ompl::base::Cost> lineCosts;
117
118 if (symmetric_)
119 {
120 lineCosts.resize(nearestNbh.size());
121 }
122
123 for (unsigned int i = 0; i < nearestNbh.size(); i++)
124 {
125 Configuration *q_near = nearestNbh.at(i);
126
127 if (q_near == q_nearest)
128 {
129 validNeighbor.at(i) = 1;
130 if (symmetric_)
131 {
132 lineCosts[i] = cost_nearest;
133 }
134 continue;
135 }
136 validNeighbor.at(i) = 0;
137
138 ompl::base::Cost line_cost = getOptimizationObjectivePtr()->motionCost(q_near->state, q_new->state);
139 ompl::base::Cost new_cost = getOptimizationObjectivePtr()->combineCosts(q_near->cost, line_cost);
140
141 if (symmetric_)
142 {
143 lineCosts[i] = line_cost;
144 }
145
146 if (getOptimizationObjectivePtr()->isCostBetterThan(new_cost, q_new->cost))
147 {
148 if ((!useKNearest_ || distance(q_near, q_new) < maxDistance_) &&
149 getBundle()->checkMotion(q_near->state, q_new->state))
150 {
151 q_new->lineCost = line_cost;
152 q_new->cost = new_cost;
153 q_new->parent = q_near;
154 validNeighbor.at(i) = 1;
155 }
156 else
157 validNeighbor.at(i) = -1;
158 }
159 }
160 //(4) Connect to minimum cost neighbor
161 addConfiguration(q_new);
162 q_new->parent->children.push_back(q_new);
163
164 bool checkForSolution = false;
165
166 // (5) Rewire the tree (if from q_new to q_near is lower cost)
167 for (unsigned int i = 0; i < nearestNbh.size(); i++)
168 {
169 Configuration *q_near = nearestNbh.at(i);
170
171 if (q_near != q_new->parent)
172 {
173 // (7a) compute cost q_new to q_near
174 base::Cost line_cost;
175 if (symmetric_)
176 {
177 line_cost = lineCosts[i];
178 }
179 else
180 {
181 line_cost = getOptimizationObjectivePtr()->motionCost(q_new->state, q_near->state);
182 }
183 base::Cost new_cost = getOptimizationObjectivePtr()->combineCosts(q_new->cost, line_cost);
184
185 // (7b) check if new cost better than q_near->cost (over old
186 // pathway)
187 if (getOptimizationObjectivePtr()->isCostBetterThan(new_cost, q_near->cost))
188 {
189 bool valid = (validNeighbor.at(i) == 1);
190 // check neighbor validity if it wasn´t checked before
191 if (validNeighbor.at(i) == 0)
192 {
193 valid = ((!useKNearest_ || distance(q_near, q_new) < maxDistance_) &&
194 getBundle()->checkMotion(q_new->state, q_near->state));
195 }
196
197 // (7c) q_new to q_near is better way to reach q_near. Remove
198 // previous connection of q_near to old parent and set it to
199 // q_new
200 // pathway)
201 if (valid)
202 {
203 // (7d) remove q_near from children of its old parent node
204 removeFromParent(q_near);
205
206 // (7g) update costs of q_near
207 q_near->lineCost = line_cost;
208 q_near->cost = new_cost;
209 q_near->parent = q_new;
210 q_near->parent->children.push_back(q_near);
211
212 // (7h) update node's children costs
213 updateChildCosts(q_near);
214 checkForSolution = true;
215 }
216 }
217 }
218 }
219
220 // (8) check if this sample satisfies the goal
221 double dist = 0.0;
222 bool satisfied = pdef_->getGoal()->isSatisfied(q_new->state, &dist);
223 if (satisfied)
224 {
225 goalConfigurations_.push_back(q_new);
226 checkForSolution = true;
227 }
228
229 if (checkForSolution)
230 {
231 bool updatedSolution = false;
232 if (!goalConfigurations_.empty() && qGoal_ == nullptr)
233 {
234 qGoal_ = q_new;
235 bestCost_ = qGoal_->cost;
236 updatedSolution = true;
237 }
238 else
239 {
240 for (unsigned int k = 0; k < goalConfigurations_.size(); k++)
241 {
243
244 if (getOptimizationObjectivePtr()->isCostBetterThan(qk->cost, bestCost_))
245 {
246 qGoal_ = qk;
247 bestCost_ = qGoal_->cost;
248 updatedSolution = true;
249 }
250 }
251 }
252 if (updatedSolution)
253 {
254 OMPL_INFORM("Found path with cost %f (level %d).", qGoal_->cost, getLevel());
255 hasSolution_ = true;
256 }
257 }
258 }
259}
260
261void ompl::multilevel::QRRTStarImpl::updateChildCosts(Configuration *q)
262{
263 for (std::size_t i = 0; i < q->children.size(); ++i)
264 {
265 q->children.at(i)->cost = getOptimizationObjectivePtr()->combineCosts(q->cost, q->children.at(i)->lineCost);
266 updateChildCosts(q->children.at(i));
267 }
268}
269
270void ompl::multilevel::QRRTStarImpl::removeFromParent(Configuration *q)
271{
272 for (auto it = q->parent->children.begin(); it != q->parent->children.end(); ++it)
273 {
274 if (*it == q)
275 {
276 q->parent->children.erase(it);
277 break;
278 }
279 }
280}
281
282bool ompl::multilevel::QRRTStarImpl::getSolution(base::PathPtr &solution)
283{
284 if (hasSolution_)
285 {
286 solutionPath_ = std::make_shared<geometric::PathGeometric>(getBundle());
287
288 Configuration *intermediate_node = qGoal_;
289 while (intermediate_node != nullptr)
290 {
291 std::static_pointer_cast<geometric::PathGeometric>(solutionPath_)->append(intermediate_node->state);
292 intermediate_node = intermediate_node->parent;
293 }
294 std::static_pointer_cast<geometric::PathGeometric>(solutionPath_)->reverse();
295 solution = solutionPath_;
296 return true;
297 }
298 else
299 {
300 return false;
301 }
302}
303
304void ompl::multilevel::QRRTStarImpl::getNearestNeighbors(Configuration *x, std::vector<Configuration *> &nearest)
305{
306 auto cardDbl = static_cast<double>(nearestDatastructure_->size() + 1u);
307
308 if (useKNearest_)
309 {
310 unsigned int k = std::ceil(k_rrt_Constant_ * log(cardDbl));
311 nearestDatastructure_->nearestK(x, k, nearest);
312 }
313 else
314 {
315 double r = std::min(maxDistance_, r_rrt_Constant_ * std::pow(log(cardDbl) / cardDbl, 1 / d_));
316 nearestDatastructure_->nearestR(x, r, nearest);
317 }
318}
319
320void ompl::multilevel::QRRTStarImpl::calculateRewiringLowerBounds()
321{
322 d_ = (double)getBundle()->getStateDimension();
323 double e = boost::math::constants::e<double>();
324 // k > 2^(d + 1) * e * (1 + 1 / d).
325 k_rrt_Constant_ = std::pow(2, d_ + 1) * e * (1.0 + 1.0 / d_);
326 // γRRG > γRRG ∗ = 2*( 1 + 1/d)^1/d * ( μ( Xfree) / ζd)^1/d
327 r_rrt_Constant_ =
328 std::pow(2 * (1.0 + 1.0 / d_) * (getBundle()->getSpaceMeasure() / unitNBallMeasure(d_)), 1.0 / d_);
329}
330
331void ompl::multilevel::QRRTStarImpl::getPlannerData(base::PlannerData &data) const
332{
333 multilevel::PlannerDataVertexAnnotated pstart(qStart_->state);
334 pstart.setLevel(getLevel());
335 data.addStartVertex(pstart);
336
337 if (hasSolution_)
338 {
339 multilevel::PlannerDataVertexAnnotated pgoal(qGoal_->state);
340 pgoal.setLevel(getLevel());
341 data.addGoalVertex(pgoal);
342 }
343
344 std::vector<Configuration *> motions;
345 if (nearestDatastructure_)
346 nearestDatastructure_->list(motions);
347
348 foreach (const Configuration *q, motions)
349 {
350 if (q->parent != nullptr)
351 {
352 multilevel::PlannerDataVertexAnnotated p1(q->parent->state);
353 multilevel::PlannerDataVertexAnnotated p2(q->state);
354 p1.setLevel(getLevel());
355 p2.setLevel(getLevel());
356 data.addEdge(p1, p2);
357 }
358 }
359
360 OMPL_DEBUG("Tree (level %d) has %d/%d vertices/edges", getLevel(), motions.size(), motions.size() - 1);
361}
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
Configuration * parent
parent index for {qrrt*}
base::Cost cost
cost to reach until current vertex in {qrrt*}
base::Cost lineCost
same as rrt*, connection cost with parent {qrrt*}
std::vector< Configuration * > children
The set of motions descending from the current motion {qrrt*}.
double maxDistance_
Maximum expansion step.
virtual Vertex addConfiguration(Configuration *q)
Add configuration to graph. Return its vertex in boost graph.
RoadmapNeighborsPtr nearestDatastructure_
Nearest neighbor structure for Bundle space configurations.
Configuration * qGoal_
The (best) goal configuration.
Configuration * xRandom_
Temporary random configuration.
Configuration * steerTowards_Range(const Configuration *from, Configuration *to)
Steer system at Configuration *from to Configuration *to, stopping if maxdistance is reached.
std::vector< Configuration * > goalConfigurations_
List of configurations that satisfy the goal condition.
base::Cost bestCost_
Best cost found so far by algorithm.
virtual void init()
Initialization methods for the first iteration (adding start configuration and doing sanity checks)
virtual double distance(const Configuration *a, const Configuration *b) const
Distance between two configurations using the current metric.
virtual bool checkMotion(const Configuration *a, const Configuration *b) const
Check if we can move from configuration a to configuration b using the current metric.
bool findSection() override
Call algorithm to solve the find section problem.
bool hasSolution_
If there exists a solution.
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
bool firstRun_
Variable to check if this bundle space planner has been run at.
unsigned int getLevel() const
Level in hierarchy of Bundle-spaces.
bool symmetric_
true if cost from a to b is same as b to a
virtual void grow() override
One iteration of RRT with adjusted sampling function.
bool useKNearest_
option to use k nn or radius
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition Console.h:70
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
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.