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 
43 ompl::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 
51 ompl::geometric::QRRTImpl::~QRRTImpl()
52 {
53  deleteConfiguration(qRandom_);
54 }
55 
56 void ompl::geometric::QRRTImpl::setGoalBias(double goalBias)
57 {
58  goalBias_ = goalBias;
59 }
60 
61 double ompl::geometric::QRRTImpl::getGoalBias() const
62 {
63  return goalBias_;
64 }
65 
66 void ompl::geometric::QRRTImpl::setRange(double maxDistance)
67 {
68  maxDistance_ = maxDistance;
69 }
70 
71 double 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 
89 bool 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 }
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
void setName(const std::string &name)
Set the name of the planner.
Definition: Planner.cpp:61
Definition of an abstract state.
Definition: State.h:114
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:124
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: QRRTImpl.cpp:84
unsigned int id_
Identity of space (to keep track of number of quotient-spaces created)
Configuration * qRandom_
Random configuration placeholder.
Definition: QRRTImpl.h:113
virtual bool sample(ompl::base::State *q_random) override
Uniform sampling.
Definition: QRRTImpl.cpp:182
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
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
double getImportance() const override
Importance based on how many vertices the tree has.
Definition: QRRTImpl.cpp:162
virtual void grow() override
One iteration of RRT with adjusted sampling function.
Definition: QRRTImpl.cpp:106
virtual bool sampleQuotient(ompl::base::State *) override
Quotient-Space sampling by choosing a random vertex from parent class tree.
Definition: QRRTImpl.cpp:204