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#include <ompl/geometric/planners/quotientspace/algorithms/QRRTImpl.h>
38#include <ompl/tools/config/SelfConfig.h>
39#include <boost/foreach.hpp>
40
41#define foreach BOOST_FOREACH
42
43ompl::geometric::QRRTImpl::QRRTImpl(const base::SpaceInformationPtr &si, QuotientSpace *parent_) : BaseT(si, parent_)
44{
45 setName("QRRTImpl" + std::to_string(id_));
46 Planner::declareParam<double>("range", this, &QRRTImpl::setRange, &QRRTImpl::getRange, "0.:1.:10000.");
47 Planner::declareParam<double>("goal_bias", this, &QRRTImpl::setGoalBias, &QRRTImpl::getGoalBias, "0.:.1:1.");
48 qRandom_ = new Configuration(Q1);
49}
50
51ompl::geometric::QRRTImpl::~QRRTImpl()
52{
53 deleteConfiguration(qRandom_);
54}
55
56void ompl::geometric::QRRTImpl::setGoalBias(double goalBias)
57{
58 goalBias_ = goalBias;
59}
60
61double ompl::geometric::QRRTImpl::getGoalBias() const
62{
63 return goalBias_;
64}
65
66void ompl::geometric::QRRTImpl::setRange(double maxDistance)
67{
68 maxDistance_ = maxDistance;
69}
70
71double ompl::geometric::QRRTImpl::getRange() const
72{
73 return maxDistance_;
74}
75
77{
78 BaseT::setup();
79 ompl::tools::SelfConfig sc(Q1, getName());
80 sc.configurePlannerRange(maxDistance_);
81 goal_ = pdef_->getGoal().get();
82}
83
85{
86 BaseT::clear();
87}
88
89bool ompl::geometric::QRRTImpl::getSolution(base::PathPtr &solution)
90{
91 if (hasSolution_)
92 {
93 bool baset_sol = BaseT::getSolution(solution);
94 if (baset_sol)
95 {
96 shortestPathVertices_ = shortestVertexPath_;
97 }
98 return baset_sol;
99 }
100 else
101 {
102 return false;
103 }
104}
105
107{
108 if (firstRun_)
109 {
110 init();
111 firstRun_ = false;
112 }
113
114 if (hasSolution_)
115 {
116 // No Goal Biasing if we already found a solution on this quotient space
117 sample(qRandom_->state);
118 }
119 else
120 {
121 double s = rng_.uniform01();
122 if (s < goalBias_)
123 {
124 Q1->copyState(qRandom_->state, qGoal_->state);
125 }
126 else
127 {
128 sample(qRandom_->state);
129 }
130 }
131
132 const Configuration *q_nearest = nearest(qRandom_);
133 double d = Q1->distance(q_nearest->state, qRandom_->state);
134 if (d > maxDistance_)
135 {
136 Q1->getStateSpace()->interpolate(q_nearest->state, qRandom_->state, maxDistance_ / d, qRandom_->state);
137 }
138
139 totalNumberOfSamples_++;
140 if (Q1->checkMotion(q_nearest->state, qRandom_->state))
141 {
142 totalNumberOfFeasibleSamples_++;
143 Configuration *q_next = new Configuration(Q1, qRandom_->state);
144 Vertex v_next = addConfiguration(q_next);
145 if (!hasSolution_)
146 {
147 // only add edge if no solution exists
148 addEdge(q_nearest->index, v_next);
149
150 double dist = 0.0;
151 bool satisfied = goal_->isSatisfied(q_next->state, &dist);
152 if (satisfied)
153 {
154 vGoal_ = addConfiguration(qGoal_);
155 addEdge(q_nearest->index, vGoal_);
156 hasSolution_ = true;
157 }
158 }
159 }
160}
161
163{
164 // Should depend on
165 // (1) level : The higher the level, the more importance
166 // (2) total samples: the more we already sampled, the less important it
167 // becomes
168 // (3) has solution: if it already has a solution, we should explore less
169 // (only when nothing happens on other levels)
170 // (4) vertices: the more vertices we have, the less important (let other
171 // levels also explore)
172 //
173 // exponentially more samples on level i. Should depend on ALL levels.
174 // const double base = 2;
175 // const double normalizer = powf(base, level);
176 // double N = (double)GetNumberOfVertices()/normalizer;
177 double N = (double)getNumberOfVertices();
178 return 1.0 / (N + 1);
179}
180
181// Make it faster by removing the validity check
183{
184 if (parent_ == nullptr)
185 {
186 Q1_sampler_->sampleUniform(q_random);
187 }
188 else
189 {
190 if (X1_dimension_ > 0)
191 {
192 X1_sampler_->sampleUniform(s_X1_tmp_);
193 parent_->sampleQuotient(s_Q0_tmp_);
194 mergeStates(s_Q0_tmp_, s_X1_tmp_, q_random);
195 }
196 else
197 {
198 parent_->sampleQuotient(q_random);
199 }
200 }
201 return true;
202}
203
205{
206 // RANDOM VERTEX SAMPLING
207 const Vertex v = boost::random_vertex(graph_, rng_boost);
208 Q1->getStateSpace()->copyState(q_random_graph, graph_[v]->state);
209 return true;
210}
Definition of an abstract state.
Definition: State.h:50
double getImportance() const override
Importance based on how many vertices the tree has.
Definition: QRRTImpl.cpp:162
virtual bool sampleQuotient(ompl::base::State *) override
Quotient-Space sampling by choosing a random vertex from parent class tree.
Definition: QRRTImpl.cpp:204
Configuration * qRandom_
Random configuration placeholder.
Definition: QRRTImpl.h:80
virtual void grow() override
One iteration of RRT with adjusted sampling function.
Definition: QRRTImpl.cpp:106
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: QRRTImpl.cpp:84
virtual bool sample(ompl::base::State *q_random) override
Uniform sampling.
Definition: QRRTImpl.cpp:182
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: QRRTImpl.cpp:76
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
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.
Definition: SelfConfig.cpp:225