Loading...
Searching...
No Matches
QRRTImpl.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019, University of Stuttgart
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 University of Stuttgart nor the names
18 * of its contributors may be used to endorse or promote products
19 * derived from this software without specific prior written
20 * permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35
36/* Author: Andreas Orthey */
37
38#include <ompl/multilevel/planners/qrrt/QRRTImpl.h>
39#include <ompl/multilevel/datastructures/graphsampler/GraphSampler.h>
40#include <ompl/tools/config/SelfConfig.h>
41#include <boost/foreach.hpp>
42
43#define foreach BOOST_FOREACH
44
45ompl::multilevel::QRRTImpl::QRRTImpl(const base::SpaceInformationPtr &si, BundleSpace *parent_) : BaseT(si, parent_)
46{
47 setName("QRRTImpl" + std::to_string(id_));
48 setImportance("exponential");
49 setGraphSampler("randomvertex");
50 getGraphSampler()->disableSegmentBias();
51}
52
53ompl::multilevel::QRRTImpl::~QRRTImpl()
54{
55}
56
58{
59 //(0) If first run, add start configuration
60 if (firstRun_)
61 {
62 init();
63 firstRun_ = false;
64
65 findSection();
66 }
67
68 //(1) Get Random Sample
69 sampleBundleGoalBias(xRandom_->state);
70
71 //(2) Get Nearest in Tree
72 const Configuration *xNearest = nearest(xRandom_);
73
74 //(3) Connect Nearest to Random (within range)
75 Configuration *xNext = extendGraphTowards_Range(xNearest, xRandom_);
76
77 //(4) If extension was successful, check if we reached goal
78 if (xNext && !hasSolution_)
79 {
80 if (isDynamic())
81 {
82 double dist;
83 bool satisfied = getGoalPtr()->isSatisfied(xNext->state, &dist);
84 if (dist < bestCost_.value())
85 {
86 bestCost_ = base::Cost(dist);
87 }
88 if (satisfied)
89 {
90 goalConfigurations_.push_back(xNext);
91 hasSolution_ = true;
92 }
93 }
94 else
95 {
96 bool satisfied = getGoalPtr()->isSatisfied(xNext->state);
97 if (satisfied)
98 {
99 goalConfigurations_.push_back(xNext);
100 hasSolution_ = true;
101 }
102 }
103 }
104}
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
virtual void grow() override
One iteration of RRT with adjusted sampling function.
Definition QRRTImpl.cpp:57